package org.recast.Detour.Source;

import org.recast.Detour.Include.*;

/**
 * @author igozha
 * @since 22.09.13 22:01
 */
public class dtNavMeshQueryImpl extends dtNavMeshQuery
{
	//
	// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
	//
	// This software is provided 'as-is', without any express or implied
	// warranty.  In no event will the authors be held liable for any damages
	// arising from the use of this software.
	// Permission is granted to anyone to use this software for any purpose,
	// including commercial applications, and to alter it and redistribute it
	// freely, subject to the following restrictions:
	// 1. The origin of this software must not be misrepresented; you must not
	//    claim that you wrote the original software. If you use this software
	//    in a product, an acknowledgment in the product documentation would be
	//    appreciated but is not required.
	// 2. Altered source versions must be plainly marked as such, and must not be
	//    misrepresented as being the original software.
	// 3. This notice may not be removed or altered from any source distribution.
	//
//
//	#include <math.h>
//	#include <float.h>
//	#include <string.h>
//	#include "DetourNavMeshQuery.h"
//	#include "DetourNavMesh.h"
//	#include "DetourNode.h"
//	#include "DetourCommon.h"
//	#include "DetourAlloc.h"
//	#include "DetourAssert.h"
//	#include <new>

	/// @class dtQueryFilter
	///
	/// <b>The Default Implementation</b>
	///
	/// At ruction: All area costs default to 1.0.  All flags are included
	/// and none are excluded.
	///
	/// If a polygon has both an include and an exclude flag, it will be excluded.
	///
	/// The way filtering works, a navigation mesh polygon must have at least one flag
	/// set to ever be considered by a query. So a polygon with no flags will never
	/// be considered.
	///
	/// Setting the include flags to 0 will result in all polygons being excluded.
	///
	/// <b>Custom Implementations</b>
	///
	/// DT_VIRTUAL_QUERYFILTER must be defined in order to extend this class.
	///
	/// Implement a custom query filter by overriding the virtual passFilter()
	/// and getCost() functions. If this is done, both functions should be as
	/// fast as possible. Use cached local copies of data rather than accessing
	/// your own objects where possible.
	///
	/// Custom implementations do not need to adhere to the flags or cost logic
	/// used by the default implementation.
	///
	/// In order for A* searches to work properly, the cost should be proportional to
	/// the travel distance. Implementing a cost modifier less than 1.0 is likely
	/// to lead to problems during pathfinding.
	///
	/// @see dtNavMeshQuery

	public dtNavMeshQueryImpl()
	{
//        m_includeFlags = 0xffff;
//        m_excludeFlags(0)
//		for (int i = 0; i < DetourNavMesh.DT_MAX_AREAS; ++i)
//			m_areaCost[i] = 1.0f;
	}

//	#ifdef DT_VIRTUAL_QUERYFILTER
	/*bool passFilter( dtPoly *//*ref*//*,
									dtMeshTile* *//*tile*//*,
								    dtPoly* poly) 
	{
		return (poly.flags & m_includeFlags) != 0 && (poly.flags & m_excludeFlags) == 0;
	}

	float getCost( float* pa,  float* pb,
								  dtPoly *//*prevRef*//*,  dtMeshTile* *//*prevTile*//*,  dtPoly* *//*prevPoly*//*,
								  dtPoly *//*curRef*//*,  dtMeshTile* *//*curTile*//*,  dtPoly* curPoly,
								  dtPoly *//*nextRef*//*,  dtMeshTile* *//*nextTile*//*,  dtPoly* *//*nextPoly*//*) 
	{
		return dtVdist(pa, pb) * m_areaCost[curPoly.getArea()];
	}*/
//	#else
//	inline bool passFilter( dtPoly /*ref*/,
//										   dtMeshTile* /*tile*/,
//										   dtPoly* poly) 
//	{
//		return (poly.flags & m_includeFlags) != 0 && (poly.flags & m_excludeFlags) == 0;
//	}
//
//	inline float getCost( float* pa,  float* pb,
//										 dtPoly /*prevRef*/,  dtMeshTile* /*prevTile*/,  dtPoly* /*prevPoly*/,
//										 dtPoly /*curRef*/,  dtMeshTile* /*curTile*/,  dtPoly* curPoly,
//										 dtPoly /*nextRef*/,  dtMeshTile* /*nextTile*/,  dtPoly* /*nextPoly*/) 
//	{
//		return dtVdist(pa, pb) * m_areaCost[curPoly.getArea()];
//	}
//	#endif

	public static float H_SCALE = 0.999f; // Search heuristic scale.

//
//	dtNavMeshQuery* dtAllocNavMeshQuery()
//	{
//		void* mem = dtAlloc(sizeof(dtNavMeshQuery), DT_ALLOC_PERM);
//		if (!mem) return 0;
//		return new(mem) dtNavMeshQuery;
//	}
//
//	void dtFreeNavMeshQuery(dtNavMeshQuery* navmesh)
//	{
//		if (!navmesh) return;
//		navmesh.~dtNavMeshQuery();
//		dtFree(navmesh);
//	}

	//////////////////////////////////////////////////////////////////////////////////////////

	/// @class dtNavMeshQuery
	///
	/// For methods that support undersized buffers, if the buffer is too small
	/// to hold the entire result set the return status of the method will include
	/// the #DT_BUFFER_TOO_SMALL flag.
	///
	/// ant member functions can be used by multiple clients without side
	/// effects. (E.g. No change to the closed list. No impact on an in-progress
	/// sliced path query. Etc.)
	///
	/// Walls and portals: A @e wall is a polygon segment that is
	/// considered impassable. A @e portal is a passable segment between polygons.
	/// A portal may be treated as a wall based on the dtQueryFilter used for a query.
	///
	/// @see dtNavMesh, dtQueryFilter, #dtAllocNavMeshQuery(), #dtAllocNavMeshQuery()

//	dtNavMeshQuery()
//	{
//        :
//        m_nav(0),
//                m_tinyNodePool(0),
//                m_nodePool(0),
//                m_openList(0)
//		memset(&m_query, 0, sizeof(dtQueryData));
//	}

//	~dtNavMeshQuery()
//	{
//		if (m_tinyNodePool)
//			m_tinyNodePool.~dtNodePool();
//		if (m_nodePool)
//			m_nodePool.~dtNodePool();
//		if (m_openList)
//			m_openList.~dtNodeQueue();
//		dtFree(m_tinyNodePool);
//		dtFree(m_nodePool);
//		dtFree(m_openList);
//	}

	/// @par
	///
	/// Must be the first function called after ruction, before other
	/// functions are used.
	///
	/// This function can be used multiple times.
	public dtStatus init(dtNavMesh nav, int maxNodes)
	{
		m_nav = nav;

		if (m_nodePool == null || m_nodePool.getMaxNodes() < maxNodes)
		{
//			if (m_nodePool)
			{
//				m_nodePool.~dtNodePool();
//				dtFree(m_nodePool);
//				m_nodePool = 0;
			}
//			m_nodePool = new (dtAlloc(sizeof(dtNodePool), DT_ALLOC_PERM)) dtNodePool(maxNodes, dtNextPow2(maxNodes/4));
			m_nodePool = new dtNodePoolImpl(maxNodes, DetourCommon.dtNextPow2(maxNodes / 4));
//			if (!m_nodePool)
//				return DT_FAILURE | DT_OUT_OF_MEMORY;
		}
		else
		{
			m_nodePool.clear();
		}

		if (m_tinyNodePool == null)
		{
			m_tinyNodePool = new dtNodePoolImpl(64, 32);
//			if (!m_tinyNodePool)
//				return DT_FAILURE | DT_OUT_OF_MEMORY;
		}
		else
		{
			m_tinyNodePool.clear();
		}

		// TODO: check the open list size too.
		if (m_openList == null || m_openList.getCapacity() < maxNodes)
		{
			if (m_openList != null)
			{
				m_openList = null;//.~dtNodeQueue();
//				dtFree(m_openList);
//				m_openList = 0;
			}
			m_openList = new dtNodeQueueImpl(maxNodes);
//			if (!m_openList)
//				return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_OUT_OF_MEMORY);
		}
		else
		{
			m_openList.clear();
		}

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	/*dtStatus findRandomPoint( dtQueryFilter* filter, float (*frand)(),
											 dtPoly* randomRef, float* randomPt) 
	{
		dtAssert(m_nav);

		// Randomly pick one tile. Assume that all tiles cover roughly the same area.
		 dtMeshTile* tile = 0;
		float tsum = 0.0f;
		for (int i = 0; i < m_nav.getMaxTiles(); i++)
		{
			 dtMeshTile* t = m_nav.getTile(i);
			if (!t || !t.header) continue;

			// Choose random tile using reservoi sampling.
			 float area = 1.0f; // Could be tile area too.
			tsum += area;
			 float u = frand();
			if (u*tsum <= area)
				tile = t;
		}
		if (!tile)
			return DT_FAILURE;

		// Randomly pick one polygon weighted by polygon area.
		 dtPoly* poly = 0;
		dtPoly polyRef = 0;
		 dtPoly base = m_nav.getPolyRefBase(tile);

		float areaSum = 0.0f;
		for (int i = 0; i < tile.header.polyCount; ++i)
		{
			 dtPoly* p = &tile.polys[i];
			// Do not return off-mesh connection polygons.
			if (p.getType() != DT_POLYTYPE_GROUND)
				continue;
			// Must pass filter
			 dtPoly ref = base | (dtPoly)i;
			if (!filter.passFilter(ref, tile, p))
				continue;

			// Calc area of the polygon.
			float polyArea = 0.0f;
			for (int j = 2; j < p.vertCount; ++j)
			{
				 float* va = &tile.verts[p.verts[0]*3];
				 float* vb = &tile.verts[p.verts[j-1]*3];
				 float* vc = &tile.verts[p.verts[j]*3];
				polyArea += dtTriArea2D(va,vb,vc);
			}

			// Choose random polygon weighted by area, using reservoi sampling.
			areaSum += polyArea;
			 float u = frand();
			if (u*areaSum <= polyArea)
			{
				poly = p;
				polyRef = ref;
			}
		}

		if (!poly)
			return DT_FAILURE;

		// Randomly pick point on polygon.
		 float* v = &tile.verts[poly.verts[0]*3];
		float verts[3*DT_VERTS_PER_POLYGON];
		float areas[DT_VERTS_PER_POLYGON];
		dtVcopy(&verts[0*3],v);
		for (int j = 1; j < poly.vertCount; ++j)
		{
			v = &tile.verts[poly.verts[j]*3];
			dtVcopy(&verts[j*3],v);
		}

		 float s = frand();
		 float t = frand();

		float pt[3];
		dtRandomPointInConvexPoly(verts, poly.vertCount, areas, s, t, pt);

		float h = 0.0f;
		dtStatus status = getPolyHeight(polyRef, pt, &h);
		if (dtStatusFailed(status))
			return status;
		pt[1] = h;

		dtVcopy(randomPt, pt);
		*randomRef = polyRef;

		return DT_SUCCESS;
	}

	dtStatus findRandomPointAroundCircle(dtPoly startRef,  float* centerPos,  float radius,
														  dtQueryFilter* filter, float (*frand)(),
														 dtPoly* randomRef, float* randomPt) 
	{
		dtAssert(m_nav);
		dtAssert(m_nodePool);
		dtAssert(m_openList);

		// Validate input
		if (!startRef || !m_nav.isValidPolyRef(startRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		 dtMeshTile* startTile = 0;
		 dtPoly* startPoly = 0;
		m_nav.getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
		if (!filter.passFilter(startRef, startTile, startPoly))
			return DT_FAILURE | DT_INVALID_PARAM;

		m_nodePool.clear();
		m_openList.clear();

		dtNode* startNode = m_nodePool.getNode(startRef);
		dtVcopy(startNode.pos, centerPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = 0;
		startNode.id = startRef;
		startNode.flags = DT_NODE_OPEN;
		m_openList.push(startNode);

		dtStatus status = DT_SUCCESS;

		 float radiusSqr = dtSqr(radius);
		float areaSum = 0.0f;

		 dtMeshTile* randomTile = 0;
		 dtPoly* randomPoly = 0;
		dtPoly randomPolyRef = 0;

		while (!m_openList.empty())
		{
			dtNode* bestNode = m_openList.pop();
			bestNode.flags &= ~DT_NODE_OPEN;
			bestNode.flags |= DT_NODE_CLOSED;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly bestRef = bestNode.id;
			 dtMeshTile* bestTile = 0;
			 dtPoly* bestPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

			// Place random locations on on ground.
			if (bestPoly.getType() == DT_POLYTYPE_GROUND)
			{
				// Calc area of the polygon.
				float polyArea = 0.0f;
				for (int j = 2; j < bestPoly.vertCount; ++j)
				{
					 float* va = &bestTile.verts[bestPoly.verts[0]*3];
					 float* vb = &bestTile.verts[bestPoly.verts[j-1]*3];
					 float* vc = &bestTile.verts[bestPoly.verts[j]*3];
					polyArea += dtTriArea2D(va,vb,vc);
				}
				// Choose random polygon weighted by area, using reservoi sampling.
				areaSum += polyArea;
				 float u = frand();
				if (u*areaSum <= polyArea)
				{
					randomTile = bestTile;
					randomPoly = bestPoly;
					randomPolyRef = bestRef;
				}
			}


			// Get parent poly and tile.
			dtPoly parentRef = 0;
			 dtMeshTile* parentTile = 0;
			 dtPoly* parentPoly = 0;
			if (bestNode.pidx)
				parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef)
				m_nav.getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

			for (unsigned int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
			{
				 dtLink* link = &bestTile.links[i];
				dtPoly neighbourRef = link.ref;
				// Skip invalid neighbours and do not follow back to parent.
				if (!neighbourRef || neighbourRef == parentRef)
					continue;

				// Expand to neighbour
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				// Do not advance if the polygon is excluded by the filter.
				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				// Find edge and calc distance to the edge.
				float va[3], vb[3];
				if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
					continue;

				// If the circle is not touching the next polygon, skip it.
				float tseg;
				float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
				if (distSqr > radiusSqr)
					continue;

				dtNode* neighbourNode = m_nodePool.getNode(neighbourRef);
				if (!neighbourNode)
				{
					status |= DT_OUT_OF_NODES;
					continue;
				}

				if (neighbourNode.flags & DT_NODE_CLOSED)
					continue;

				// Cost
				if (neighbourNode.flags == 0)
					dtVlerp(neighbourNode.pos, va, vb, 0.5f);

				 float total = bestNode.total + dtVdist(bestNode.pos, neighbourNode.pos);

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_OPEN) && total >= neighbourNode.total)
					continue;

				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.total = total;

				if (neighbourNode.flags & DT_NODE_OPEN)
				{
					m_openList.modify(neighbourNode);
				}
				else
				{
					neighbourNode.flags = DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}
			}
		}

		if (!randomPoly)
			return DT_FAILURE;

		// Randomly pick point on polygon.
		 float* v = &randomTile.verts[randomPoly.verts[0]*3];
		float verts[3*DT_VERTS_PER_POLYGON];
		float areas[DT_VERTS_PER_POLYGON];
		dtVcopy(&verts[0*3],v);
		for (int j = 1; j < randomPoly.vertCount; ++j)
		{
			v = &randomTile.verts[randomPoly.verts[j]*3];
			dtVcopy(&verts[j*3],v);
		}

		 float s = frand();
		 float t = frand();

		float pt[3];
		dtRandomPointInConvexPoly(verts, randomPoly.vertCount, areas, s, t, pt);

		float h = 0.0f;
		dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
		if (dtStatusFailed(status))
			return stat;
		pt[1] = h;

		dtVcopy(randomPt, pt);
		*randomRef = randomPolyRef;

		return DT_SUCCESS;
	}*/

	//////////////////////////////////////////////////////////////////////////////////////////

	/// @par
	///
	/// Uses the detail polygons to find the surface height. (Most accurate.)
	///
	/// @p pos does not have to be within the bounds of the polygon or navigation mesh.
	///
	/// See closestPointOnPolyBoundary() for a limited but faster option.
	///
	public dtStatus closestPointOnPoly(dtPoly ref, float[] pos, float[] closest)
	{
//		dtAssert(m_nav);
		dtMeshTile[] tile = new dtMeshTile[1];
		dtPoly[] poly = new dtPoly[1];
		if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(ref, tile, poly)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		if (tile[0] == null)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		closestPointOnPolyInTile(tile[0], poly[0], pos, closest);

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	public void closestPointOnPolyInTile(dtMeshTile tile, dtPoly poly,
										 float[] pos, float[] closest)
	{
		// Off-mesh connections don't have detail polygons.
		if (poly.getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
		{
//			 float* v0 = &tile.verts[poly.verts[0]*3];
//			 float* v1 = &tile.verts[poly.verts[1]*3];
			float d0 = DetourCommon.dtVdist(pos, 0, tile.verts, poly.verts[0] * 3);
			float d1 = DetourCommon.dtVdist(pos, 0, tile.verts, poly.verts[1] * 3);
			float u = d0 / (d0 + d1);
			DetourCommon.dtVlerp(closest, tile.verts, poly.verts[0] * 3, tile.verts, poly.verts[1] * 3, u);
			return;
		}

//		 int ip = ( int)(poly - tile.polys);
		int ip = -1;
		dtPolyDetail pd = tile.detailMeshes[ip];

		// Clamp point to be inside the polygon.
		float verts[] = new float[DetourNavMesh.DT_VERTS_PER_POLYGON * 3];
		float edged[] = new float[DetourNavMesh.DT_VERTS_PER_POLYGON];
		float edget[] = new float[DetourNavMesh.DT_VERTS_PER_POLYGON];
		int nv = poly.vertCount;
		for (int i = 0; i < nv; ++i)
		{
			DetourCommon.dtVcopy(verts, i * 3, tile.verts, poly.verts[i] * 3);
		}

		DetourCommon.dtVcopy(closest, pos);
		if (!new DetourCommonImpl().dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
		{
			// Point is outside the polygon, dtClamp to nearest edge.
			float dmin = Float.MAX_VALUE;
			int imin = -1;
			for (int i = 0; i < nv; ++i)
			{
				if (edged[i] < dmin)
				{
					dmin = edged[i];
					imin = i;
				}
			}
//			 float* va = &verts[imin*3];
//			 float* vb = &verts[((imin+1)%nv)*3];
			DetourCommon.dtVlerp(closest, verts, imin * 3, verts, ((imin + 1) % nv) * 3, edget[imin]);
		}

		// Find height at the location.
		for (int j = 0; j < pd.triCount; ++j)
		{
			char[] t = tile.detailTris;
			int tIndex = (pd.triBase + j) * 4;
			float v[][] = new float[3][];
			int vIndexes[] = new int[3];
			for (int k = 0; k < 3; ++k)
			{
				if (t[tIndex + k] < poly.vertCount)
				{
					vIndexes[k] = poly.verts[t[tIndex + k]] * 3;
					v[k] = tile.verts;
				}
				else
				{
					vIndexes[k] = (pd.vertBase + (t[tIndex + k] - poly.vertCount)) * 3;
					v[k] = tile.detailVerts;
				}
			}
			float h[] = new float[1];
			if (new DetourCommonImpl().dtClosestHeightPointTriangle(pos, v[0], vIndexes[0], v[1], vIndexes[1], v[2], vIndexes[2], h))
			{
				closest[1] = h[0];
				break;
			}
		}


	}

	/// @par
	///
	/// Much faster than closestPointOnPoly().
	///
	/// If the provided position lies within the polygon's xz-bounds (above or below),
	/// then @p pos and @p closest will be equal.
	///
	/// The height of @p closest will be the polygon boundary.  The height detail is not used.
	///
	/// @p pos does not have to be within the bounds of the polybon or the navigation mesh.
	///
	public dtStatus closestPointOnPolyBoundary(dtPoly ref, float[] pos, float[] closest)
	{
//		dtAssert(m_nav);

		dtMeshTile[] tile = new dtMeshTile[1];
		dtPoly[] poly = new dtPoly[1];
		if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(ref, tile, poly)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		// Collect vertices.
		float[] verts = new float[DetourNavMesh.DT_VERTS_PER_POLYGON * 3];
		float[] edged = new float[DetourNavMesh.DT_VERTS_PER_POLYGON];
		float[] edget = new float[DetourNavMesh.DT_VERTS_PER_POLYGON];
		int nv = 0;
		for (int i = 0; i < (int)poly[0].vertCount; ++i)
		{
			DetourCommon.dtVcopy(verts, nv * 3, tile[0].verts, poly[0].verts[i] * 3);
			nv++;
		}

		boolean inside = new DetourCommonImpl().dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
		if (inside)
		{
			// Point is inside the polygon, return the point.
			DetourCommon.dtVcopy(closest, pos);
		}
		else
		{
			// Point is outside the polygon, dtClamp to nearest edge.
			float dmin = Float.MAX_VALUE;
			int imin = -1;
			for (int i = 0; i < nv; ++i)
			{
				if (edged[i] < dmin)
				{
					dmin = edged[i];
					imin = i;
				}
			}
//			 float* va = &verts[imin*3];
//			 float* vb = &verts[((imin+1)%nv)*3];
			DetourCommon.dtVlerp(closest, verts, imin * 3, verts, ((imin + 1) % nv) * 3, edget[imin]);
		}

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	/// @par
	///
	/// Will return #DT_FAILURE if the provided position is outside the xz-bounds
	/// of the polygon.
	///
	public dtStatus getPolyHeight(dtPoly ref, float[] pos, float[] height)
	{
//		dtAssert(m_nav);

		dtMeshTile tile[] = new dtMeshTile[1];
		dtPoly[] poly = new dtPoly[1];
		if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(ref, tile, poly)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		if (poly[0].getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
		{
			float[] v0 = tile[0].verts;
			int v0Index = poly[0].verts[0] * 3;
			float[] v1 = tile[0].verts;
			int v1Index = poly[0].verts[1] * 3;
			float d0 = DetourCommon.dtVdist(pos, 0, v0, v0Index);
			float d1 = DetourCommon.dtVdist(pos, 0, v1, v1Index);
			float u = d0 / (d0 + d1);
			if (height != null)
				height[0] = v0[v0Index + 1] + (v1[v1Index + 1] - v0[v0Index + 1]) * u;
			return new dtStatus(dtStatus.DT_SUCCESS);
		}
		else
		{
			int ip = -1;//(int)(poly - tile[0].polys);
			for (int i = 0; i < tile[0].polys.length; i++)
			{
				if (tile[0].polys[i] == poly[0])
				{
					ip = i;
				}
			}
			dtPolyDetail pd = tile[0].detailMeshes[ip];
			for (int j = 0; j < pd.triCount; ++j)
			{
				char[] t = tile[0].detailTris;
				int tIndex = (pd.triBase + j) * 4;
				float v[] = new float[3];
				for (int k = 0; k < 3; ++k)
				{
					if (t[tIndex + k] < poly[0].vertCount)
						v[k] = tile[0].verts[poly[0].verts[t[tIndex + k]] * 3];
					else
						v[k] = tile[0].detailVerts[(pd.vertBase + (t[tIndex + k] - poly[0].vertCount)) * 3];
				}
				float[] h = new float[1];
				if (new DetourCommonImpl().dtClosestHeightPointTriangle(pos, v, 0, v, 1, v, 2, h))
				{
					if (height != null)
						height[0] = h[0];
					return new dtStatus(dtStatus.DT_SUCCESS);
				}
			}
		}

		return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
	}

	/// @par
	///
	/// @note If the search box does not intersect any polygons the search will
	/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check
	/// @p nearestRef before using @p nearestPt.
	///
	/// @warning This function is not suitable for large area searches.  If the search
	/// extents overlaps more than 128 polygons it may return an invalid result.
	///
	public dtStatus findNearestPoly(float[] center, float[] extents,
									dtQueryFilter filter,
									dtPoly[] nearestRef, float[] nearestPt)
	{
//		dtAssert(m_nav);

		nearestRef[0] = null;

		// Get nearby polygons from proximity grid.
		dtPoly polys[] = new dtPoly[128];
		int[] polyCount = new int[]{0};
		if (dtStatus.dtStatusFailed(queryPolygons(center, extents, filter, polys, polyCount, 128)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		// Find nearest polygon amongst the nearby polygons.
		dtPoly nearest = null;
		float nearestDistanceSqr = Float.MAX_VALUE;
		for (int i = 0; i < polyCount[0]; ++i)
		{
			dtPoly ref = polys[i];
			float closestPtPoly[] = new float[3];
			closestPointOnPoly(ref, center, closestPtPoly);
			float d = DetourCommon.dtVdistSqr(center, closestPtPoly);
			if (d < nearestDistanceSqr)
			{
				if (nearestPt != null)
					DetourCommon.dtVcopy(nearestPt, closestPtPoly);
				nearestDistanceSqr = d;
				nearest = ref;
			}
		}

		if (nearestRef != null)
		{
			nearestRef[0] = nearest;
		}

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	/*dtPoly findNearestPolyInTile( dtMeshTile* tile,  float* center,  float* extents,
													 dtQueryFilter* filter, float* nearestPt) 
	{
		dtAssert(m_nav);

		float bmin[3], bmax[3];
		dtVsub(bmin, center, extents);
		dtVadd(bmax, center, extents);

		// Get nearby polygons from proximity grid.
		dtPoly polys[128];
		int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);

		// Find nearest polygon amongst the nearby polygons.
		dtPoly nearest = 0;
		float nearestDistanceSqr = FLT_MAX;
		for (int i = 0; i < polyCount; ++i)
		{
			dtPoly ref = polys[i];
			 dtPoly* poly = &tile.polys[m_nav.decodePolyIdPoly(ref)];
			float closestPtPoly[3];
			closestPointOnPolyInTile(tile, poly, center, closestPtPoly);

			float d = dtVdistSqr(center, closestPtPoly);
			if (d < nearestDistanceSqr)
			{
				if (nearestPt)
					dtVcopy(nearestPt, closestPtPoly);
				nearestDistanceSqr = d;
				nearest = ref;
			}
		}

		return nearest;
	} */

	public int queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax,
								   dtQueryFilter filter,
								   dtPoly[] polys, int polysIndex, int maxPolys)
	{
//		dtAssert(m_nav);

		if (tile.bvTree != null)
		{
			int nodeIndex = 0;
			dtBVNode node = tile.bvTree[nodeIndex];
//			dtBVNode end = tile.bvTree[tile.header.bvNodeCount];
			float[] tbmin = tile.header.bmin;
			float[] tbmax = tile.header.bmax;
			float qfac = tile.header.bvQuantFactor;

			// Calculate quantized box
			int bmin[] = new int[3], bmax[] = new int[3];
			// dtClamp query box to world box.
			float minx = DetourCommon.dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
			float miny = DetourCommon.dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
			float minz = DetourCommon.dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
			float maxx = DetourCommon.dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
			float maxy = DetourCommon.dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
			float maxz = DetourCommon.dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
			// Quantize
			bmin[0] = (int)(qfac * minx) & 0xfffe;
			bmin[1] = (int)(qfac * miny) & 0xfffe;
			bmin[2] = (int)(qfac * minz) & 0xfffe;
			bmax[0] = (int)(qfac * maxx + 1) | 1;
			bmax[1] = (int)(qfac * maxy + 1) | 1;
			bmax[2] = (int)(qfac * maxz + 1) | 1;

			// Traverse tree
			dtPoly base = m_nav.getPolyRefBase(tile);
			int n = 0;
			while (nodeIndex < tile.header.bvNodeCount)
			{
				boolean overlap = DetourCommon.dtOverlapQuantBounds(bmin, bmax, node.bmin, node.bmax);
				boolean isLeafNode = node.i >= 0;

				if (isLeafNode && overlap)
				{
					dtPoly ref = null;//base | (dtPoly)node.i;
					if (filter.passFilter(ref, tile, tile.polys[node.i]))
					{
						if (n < maxPolys)
							polys[n++] = ref;
					}
				}

				if (overlap || isLeafNode)
				{
					nodeIndex++;
					if (nodeIndex == tile.bvTree.length)
					{
						node = null;
					}
					else
					{
						node = tile.bvTree[nodeIndex];
					}
//					node++;
				}
				else
				{
					int escapeIndex = -node.i;
					nodeIndex += escapeIndex;
					node = tile.bvTree[nodeIndex];
				}
			}

			return n;
		}
		else
		{
			float bmin[] = new float[3], bmax[] = new float[3];
			int n = 0;
			dtPoly base = m_nav.getPolyRefBase(tile);
			for (int i = 0; i < tile.header.polyCount; ++i)
			{
				dtPoly p = tile.polys[i];
				// Do not return off-mesh connection polygons.
				if (p.getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
					continue;
				// Must pass filter
				dtPoly ref = null;//base | (dtPoly)i;
				if (!filter.passFilter(ref, tile, p))
					continue;
				// Calc polygon bounds.
//				 float* v = &tile.verts[p.verts[0]*3];
				DetourCommon.dtVcopy(bmin, 0, tile.verts, p.verts[0] * 3);
				DetourCommon.dtVcopy(bmax, 0, tile.verts, p.verts[0] * 3);
				for (int j = 1; j < p.vertCount; ++j)
				{
//					v = tile.verts[p.verts[j]*3];
					DetourCommon.dtVmin(bmin, 0, tile.verts, p.verts[j] * 3);
					DetourCommon.dtVmax(bmax, 0, tile.verts, p.verts[j] * 3);
				}
				if (DetourCommon.dtOverlapBounds(qmin, qmax, bmin, bmax))
				{
					if (n < maxPolys)
						polys[n++] = ref;
				}
			}
			return n;
		}
	}

	/// @par
	///
	/// If no polygons are found, the function will return #DT_SUCCESS with a
	/// @p polyCount of zero.
	///
	/// If @p polys is too small to hold the entire result set, then the array will
	/// be filled to capacity. The method of choosing which polygons from the
	/// full set are included in the partial result set is undefined.
	///
	public dtStatus queryPolygons(float[] center, float[] extents,
								  dtQueryFilter filter,
								  dtPoly[] polys, int[] polyCount, int maxPolys)
	{
//		dtAssert(m_nav);

		float bmin[] = new float[3], bmax[] = new float[3];
		DetourCommon.dtVsub(bmin, center, extents);
		DetourCommon.dtVadd(bmax, center, extents);

		// Find tiles the query touches.
		int[] minx = new int[1];
		int miny[] = new int[1];
		int[] maxx = new int[1];
		int maxy[] = new int[1];
		m_nav.calcTileLoc(bmin, minx, miny);
		m_nav.calcTileLoc(bmax, maxx, maxy);

		int MAX_NEIS = 32;
		dtMeshTile neis[] = new dtMeshTile[MAX_NEIS];

		int n = 0;
		for (int y = miny[0]; y <= maxy[0]; ++y)
		{
			for (int x = minx[0]; x <= maxx[0]; ++x)
			{
				int nneis = m_nav.getTilesAt(x, y, neis, MAX_NEIS);
				for (int j = 0; j < nneis; ++j)
				{
					n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys, n, maxPolys - n);
					if (n >= maxPolys)
					{
						polyCount[0] = n;
						return new dtStatus(dtStatus.DT_SUCCESS | dtStatus.DT_BUFFER_TOO_SMALL);
					}
				}
			}
		}
		polyCount[0] = n;

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	/// @par
	///
	/// If the end polygon cannot be reached through the navigation graph,
	/// the last polygon in the path will be the nearest the end polygon.
	///
	/// If the path array is to small to hold the full result, it will be filled as
	/// far as possible from the start polygon toward the end polygon.
	///
	/// The start and end positions are used to calculate traversal costs.
	/// (The y-values impact the result.)
	///
	/*dtStatus findPath(dtPoly startRef, dtPoly endRef,
									   float* startPos,  float* endPos,
									   dtQueryFilter* filter,
									  dtPoly* path, int* pathCount,  int maxPath) 
	{
		dtAssert(m_nav);
		dtAssert(m_nodePool);
		dtAssert(m_openList);

		*pathCount = 0;

		if (!startRef || !endRef)
			return DT_FAILURE | DT_INVALID_PARAM;

		if (!maxPath)
			return DT_FAILURE | DT_INVALID_PARAM;

		// Validate input
		if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		if (startRef == endRef)
		{
			path[0] = startRef;
			*pathCount = 1;
			return DT_SUCCESS;
		}

		m_nodePool.clear();
		m_openList.clear();

		dtNode* startNode = m_nodePool.getNode(startRef);
		dtVcopy(startNode.pos, startPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = dtVdist(startPos, endPos) * H_SCALE;
		startNode.id = startRef;
		startNode.flags = DT_NODE_OPEN;
		m_openList.push(startNode);

		dtNode* lastBestNode = startNode;
		float lastBestNodeCost = startNode.total;

		dtStatus status = DT_SUCCESS;

		while (!m_openList.empty())
		{
			// Remove node from open list and put it in closed list.
			dtNode* bestNode = m_openList.pop();
			bestNode.flags &= ~DT_NODE_OPEN;
			bestNode.flags |= DT_NODE_CLOSED;

			// Reached the goal, stop searching.
			if (bestNode.id == endRef)
			{
				lastBestNode = bestNode;
				break;
			}

			// Get current poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly bestRef = bestNode.id;
			 dtMeshTile* bestTile = 0;
			 dtPoly* bestPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

			// Get parent poly and tile.
			dtPoly parentRef = 0;
			 dtMeshTile* parentTile = 0;
			 dtPoly* parentPoly = 0;
			if (bestNode.pidx)
				parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef)
				m_nav.getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

			for (unsigned int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
			{
				dtPoly neighbourRef = bestTile.links[i].ref;

				// Skip invalid ids and do not expand back to where we came from.
				if (!neighbourRef || neighbourRef == parentRef)
					continue;

				// Get neighbour poly and tile.
				// The API input has been cheked already, skip checking internal data.
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				dtNode* neighbourNode = m_nodePool.getNode(neighbourRef);
				if (!neighbourNode)
				{
					status |= DT_OUT_OF_NODES;
					continue;
				}

				// If the node is visited the first time, calculate node position.
				if (neighbourNode.flags == 0)
				{
					getEdgeMidPoint(bestRef, bestPoly, bestTile,
									neighbourRef, neighbourPoly, neighbourTile,
									neighbourNode.pos);
				}

				// Calculate cost and heuristic.
				float cost = 0;
				float heuristic = 0;

				// Special case for last node.
				if (neighbourRef == endRef)
				{
					// Cost
					 float curCost = filter.getCost(bestNode.pos, neighbourNode.pos,
														  parentRef, parentTile, parentPoly,
														  bestRef, bestTile, bestPoly,
														  neighbourRef, neighbourTile, neighbourPoly);
					 float endCost = filter.getCost(neighbourNode.pos, endPos,
														  bestRef, bestTile, bestPoly,
														  neighbourRef, neighbourTile, neighbourPoly,
														  0, 0, 0);

					cost = bestNode.cost + curCost + endCost;
					heuristic = 0;
				}
				else
				{
					// Cost
					 float curCost = filter.getCost(bestNode.pos, neighbourNode.pos,
														  parentRef, parentTile, parentPoly,
														  bestRef, bestTile, bestPoly,
														  neighbourRef, neighbourTile, neighbourPoly);
					cost = bestNode.cost + curCost;
					heuristic = dtVdist(neighbourNode.pos, endPos)*H_SCALE;
				}

				 float total = cost + heuristic;

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_OPEN) && total >= neighbourNode.total)
					continue;
				// The node is already visited and process, and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_CLOSED) && total >= neighbourNode.total)
					continue;

				// Add or update the node.
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
				neighbourNode.cost = cost;
				neighbourNode.total = total;

				if (neighbourNode.flags & DT_NODE_OPEN)
				{
					// Already in open, update node location.
					m_openList.modify(neighbourNode);
				}
				else
				{
					// Put the node in open list.
					neighbourNode.flags |= DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}

				// Update nearest node to target so far.
				if (heuristic < lastBestNodeCost)
				{
					lastBestNodeCost = heuristic;
					lastBestNode = neighbourNode;
				}
			}
		}

		if (lastBestNode.id != endRef)
			status |= DT_PARTIAL_RESULT;

		// Reverse the path.
		dtNode* prev = 0;
		dtNode* node = lastBestNode;
		do
		{
			dtNode* next = m_nodePool.getNodeAtIdx(node.pidx);
			node.pidx = m_nodePool.getNodeIdx(prev);
			prev = node;
			node = next;
		}
		while (node);

		// Store path
		node = prev;
		int n = 0;
		do
		{
			path[n++] = node.id;
			if (n >= maxPath)
			{
				status |= DT_BUFFER_TOO_SMALL;
				break;
			}
			node = m_nodePool.getNodeAtIdx(node.pidx);
		}
		while (node);

		*pathCount = n;

		return status;
	}*/

	/// @par
	///
	/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
	/// or finalizeSlicedFindPathPartial() may result in corrupted data!
	///
	/// The @p filter pointer is stored and used for the duration of the sliced
	/// path query.
	///
	public dtStatus initSlicedFindPath(dtPoly startRef, dtPoly endRef,
									   float[] startPos, float[] endPos,
									   dtQueryFilter filter)
	{
//		dtAssert(m_nav);
//		dtAssert(m_nodePool);
//		dtAssert(m_openList);

		// Init path state.
//		memset(&m_query, 0, sizeof(dtQueryData));
		m_query = null;
		m_query.status = new dtStatus(dtStatus.DT_FAILURE);
		m_query.startRef = startRef;
		m_query.endRef = endRef;
		DetourCommon.dtVcopy(m_query.startPos, startPos);
		DetourCommon.dtVcopy(m_query.endPos, endPos);
		m_query.filter = filter;

		if (startRef == null || endRef == null)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		// Validate input
		if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		if (startRef == endRef)
		{
			m_query.status = new dtStatus(dtStatus.DT_SUCCESS);
			return new dtStatus(dtStatus.DT_SUCCESS);
		}

		m_nodePool.clear();
		m_openList.clear();

		dtNode startNode = m_nodePool.getNode(startRef);
		DetourCommon.dtVcopy(startNode.pos, startPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = DetourCommon.dtVdist(startPos, endPos) * H_SCALE;
		startNode.id = startRef;
		startNode.flags = dtNodeFlags.DT_NODE_OPEN;
		m_openList.push(startNode);

		m_query.status = new dtStatus(dtStatus.DT_IN_PROGRESS);
		m_query.lastBestNode = startNode;
		m_query.lastBestNodeCost = startNode.total;

		return m_query.status;
	}

	public dtStatus updateSlicedFindPath(int maxIter, int[] doneIters)
	{
		if (!dtStatus.dtStatusInProgress(m_query.status))
			return m_query.status;

		// Make sure the request is still valid.
		if (!m_nav.isValidPolyRef(m_query.startRef) || !m_nav.isValidPolyRef(m_query.endRef))
		{
			m_query.status = new dtStatus(dtStatus.DT_FAILURE);
			return new dtStatus(dtStatus.DT_FAILURE);
		}

		int iter = 0;
		while (iter < maxIter && !m_openList.empty())
		{
			iter++;

			// Remove node from open list and put it in closed list.
			dtNode bestNode = m_openList.pop();
			bestNode.flags &= ~dtNodeFlags.DT_NODE_OPEN;
			bestNode.flags |= dtNodeFlags.DT_NODE_CLOSED;

			// Reached the goal, stop searching.
			if (bestNode.id == m_query.endRef)
			{
				m_query.lastBestNode = bestNode;
				dtStatus details = new dtStatus(m_query.status.dtStatus & dtStatus.DT_STATUS_DETAIL_MASK);
				m_query.status = new dtStatus(dtStatus.DT_SUCCESS | details.dtStatus);
				if (doneIters != null)
					doneIters[0] = iter;
				return m_query.status;
			}

			// Get current poly and tile.
			// The API input has been cheked already, skip checking internal data.
			dtPoly bestRef = bestNode.id;
			dtMeshTile[] bestTile = new dtMeshTile[1];
			dtPoly[] bestPoly = new dtPoly[1];
			if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(bestRef, bestTile, bestPoly)))
			{
				// The polygon has disappeared during the sliced query, fail.
				m_query.status = new dtStatus(dtStatus.DT_FAILURE);
				if (doneIters != null)
					doneIters[0] = iter;
				return m_query.status;
			}

			// Get parent poly and tile.
			dtPoly[] parentRef = new dtPoly[1];
			dtMeshTile parentTile[] = new dtMeshTile[1];
			dtPoly[] parentPoly = new dtPoly[1];
			if (bestNode.pidx != 0)
				parentRef[0] = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef[0] != null)
			{
				if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(parentRef[0], parentTile, parentPoly)))
				{
					// The polygon has disappeared during the sliced query, fail.
					m_query.status = new dtStatus(dtStatus.DT_FAILURE);
					if (doneIters != null)
						doneIters[0] = iter;
					return m_query.status;
				}
			}

			for (int i = bestPoly[0].firstLink; i != DetourNavMesh.DT_NULL_LINK; i = bestTile[0].links[i].next)
			{
				dtPoly neighbourRef = bestTile[0].links[i].ref;

				// Skip invalid ids and do not expand back to where we came from.
				if (neighbourRef == null || neighbourRef == parentRef[0])
					continue;

				// Get neighbour poly and tile.
				// The API input has been cheked already, skip checking internal data.
				dtMeshTile[] neighbourTile = new dtMeshTile[1];
				dtPoly neighbourPoly[] = new dtPoly[1];
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, neighbourTile, neighbourPoly);

				if (!m_query.filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				dtNode neighbourNode = m_nodePool.getNode(neighbourRef);
				if (neighbourNode == null)
				{
					m_query.status.dtStatus |= dtStatus.DT_OUT_OF_NODES;
					continue;
				}

				// If the node is visited the first time, calculate node position.
				if (neighbourNode.flags == 0)
				{
					getEdgeMidPoint(bestRef, bestPoly[0], bestTile[0],
									neighbourRef, neighbourPoly[0], neighbourTile[0],
									neighbourNode.pos);
				}

				// Calculate cost and heuristic.
				float cost = 0;
				float heuristic = 0;

				// Special case for last node.
				if (neighbourRef == m_query.endRef)
				{
					// Cost
					float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos,
														   parentRef[0], parentTile[0], parentPoly[0],
														   bestRef, bestTile[0], bestPoly[0],
														   neighbourRef, neighbourTile[0], neighbourPoly[0]);
					float endCost = m_query.filter.getCost(neighbourNode.pos, m_query.endPos,
														   bestRef, bestTile[0], bestPoly[0],
														   neighbourRef, neighbourTile[0], neighbourPoly[0],
														   null, null, null);

					cost = bestNode.cost + curCost + endCost;
					heuristic = 0;
				}
				else
				{
					// Cost
					float curCost = m_query.filter.getCost(bestNode.pos, neighbourNode.pos,
														   parentRef[0], parentTile[0], parentPoly[0],
														   bestRef, bestTile[0], bestPoly[0],
														   neighbourRef, neighbourTile[0], neighbourPoly[0]);
					cost = bestNode.cost + curCost;
					heuristic = DetourCommon.dtVdist(neighbourNode.pos, m_query.endPos) * H_SCALE;
				}

				float total = cost + heuristic;

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & dtNodeFlags.DT_NODE_OPEN) != 0 && total >= neighbourNode.total)
					continue;
				// The node is already visited and process, and the new result is worse, skip.
				if ((neighbourNode.flags & dtNodeFlags.DT_NODE_CLOSED) != 0 && total >= neighbourNode.total)
					continue;

				// Add or update the node.
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~dtNodeFlags.DT_NODE_CLOSED);
				neighbourNode.cost = cost;
				neighbourNode.total = total;

				if ((neighbourNode.flags & dtNodeFlags.DT_NODE_OPEN) != 0)
				{
					// Already in open, update node location.
					m_openList.modify(neighbourNode);
				}
				else
				{
					// Put the node in open list.
					neighbourNode.flags |= dtNodeFlags.DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}

				// Update nearest node to target so far.
				if (heuristic < m_query.lastBestNodeCost)
				{
					m_query.lastBestNodeCost = heuristic;
					m_query.lastBestNode = neighbourNode;
				}
			}
		}

		// Exhausted all nodes, but could not find path.
		if (m_openList.empty())
		{
			dtStatus details = new dtStatus(m_query.status.dtStatus & dtStatus.DT_STATUS_DETAIL_MASK);
			m_query.status = new dtStatus(dtStatus.DT_SUCCESS | details.dtStatus);
		}

		if (doneIters != null)
			doneIters[0] = iter;

		return m_query.status;
	}

	public dtStatus finalizeSlicedFindPath(dtPoly[] path, int[] pathCount, int maxPath)
	{
		pathCount[0] = 0;

		if (dtStatus.dtStatusFailed(m_query.status))
		{
			// Reset query.
//			memset(&m_query, 0, sizeof(dtQueryData));
			m_query = null;
			return new dtStatus(dtStatus.DT_FAILURE);
		}

		int n = 0;

		if (m_query.startRef == m_query.endRef)
		{
			// Special case: the search starts and ends at same poly.
			path[n++] = m_query.startRef;
		}
		else
		{
			// Reverse the path.
//			dtAssert(m_query.lastBestNode);

			if (m_query.lastBestNode.id != m_query.endRef)
				m_query.status.dtStatus |= dtStatus.DT_PARTIAL_RESULT;

			dtNode prev = null;
			dtNode node = m_query.lastBestNode;
			do
			{
				dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
				node.pidx = m_nodePool.getNodeIdx(prev);
				prev = node;
				node = next;
			}
			while (node != null);

			// Store path
			node = prev;
			do
			{
				path[n++] = node.id;
				if (n >= maxPath)
				{
					m_query.status.dtStatus |= dtStatus.DT_BUFFER_TOO_SMALL;
					break;
				}
				node = m_nodePool.getNodeAtIdx(node.pidx);
			}
			while (node != null);
		}

		dtStatus details = new dtStatus(m_query.status.dtStatus & dtStatus.DT_STATUS_DETAIL_MASK);

		// Reset query.
//		memset(&m_query, 0, sizeof(dtQueryData));
		m_query = null;

		pathCount[0] = n;

		return new dtStatus(dtStatus.DT_SUCCESS | details.dtStatus);
	}

	public dtStatus finalizeSlicedFindPathPartial(dtPoly[] existing, int existingSize,
												  dtPoly[] path, int[] pathCount, int maxPath)
	{
		pathCount[0] = 0;

		if (existingSize == 0)
		{
			return new dtStatus(dtStatus.DT_FAILURE);
		}

		if (dtStatus.dtStatusFailed(m_query.status))
		{
			// Reset query.
//			memset(&m_query, 0, sizeof(dtQueryData));
			m_query = null;
			return new dtStatus(dtStatus.DT_FAILURE);
		}

		int n = 0;

		if (m_query.startRef == m_query.endRef)
		{
			// Special case: the search starts and ends at same poly.
			path[n++] = m_query.startRef;
		}
		else
		{
			// Find furthest existing node that was visited.
			dtNode prev = null;
			dtNode node = null;
			for (int i = existingSize - 1; i >= 0; --i)
			{
				node = m_nodePool.findNode(existing[i]);
				if (node != null)
					break;
			}

			if (node == null)
			{
				m_query.status.dtStatus |= dtStatus.DT_PARTIAL_RESULT;
//				dtAssert(m_query.lastBestNode);
				node = m_query.lastBestNode;
			}

			// Reverse the path.
			do
			{
				dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
				node.pidx = m_nodePool.getNodeIdx(prev);
				prev = node;
				node = next;
			}
			while (node != null);

			// Store path
			node = prev;
			do
			{
				path[n++] = node.id;
				if (n >= maxPath)
				{
					m_query.status.dtStatus |= dtStatus.DT_BUFFER_TOO_SMALL;
					break;
				}
				node = m_nodePool.getNodeAtIdx(node.pidx);
			}
			while (node != null);
		}

		dtStatus details = new dtStatus(m_query.status.dtStatus & dtStatus.DT_STATUS_DETAIL_MASK);

		// Reset query.
//		memset(&m_query, 0, sizeof(dtQueryData));
		m_query = null;

		pathCount[0] = n;

		return new dtStatus(dtStatus.DT_SUCCESS | details.dtStatus);
	}

	public dtStatus appendVertex(float[] pos, char flags, dtPoly ref,
								 float[] straightPath, char[] straightPathFlags, dtPoly[] straightPathRefs,
								 int[] straightPathCount, int maxStraightPath)
	{
		if ((straightPathCount[0]) > 0 && DetourCommon.dtVequal(straightPath, ((straightPathCount[0]) - 1) * 3, pos, 0))
		{
			// The vertices are equal, update flags and poly.
			if (straightPathFlags != null)
				straightPathFlags[(straightPathCount[0]) - 1] = flags;
			if (straightPathRefs != null)
				straightPathRefs[(straightPathCount[0]) - 1] = ref;
		}
		else
		{
			// Append new vertex.
			DetourCommon.dtVcopy(straightPath, (straightPathCount[0]) * 3, pos, 0);
			if (straightPathFlags != null)
				straightPathFlags[(straightPathCount[0])] = flags;
			if (straightPathRefs != null)
				straightPathRefs[(straightPathCount[0])] = ref;
			(straightPathCount[0])++;
			// If reached end of path or there is no space to append more vertices, return.
			if (flags == dtStraightPathFlags.DT_STRAIGHTPATH_END || (straightPathCount[0]) >= maxStraightPath)
			{
				return new dtStatus(dtStatus.DT_SUCCESS | (((straightPathCount[0]) >= maxStraightPath) ? dtStatus.DT_BUFFER_TOO_SMALL : 0));
			}
		}
		return new dtStatus(dtStatus.DT_IN_PROGRESS);
	}

	public dtStatus appendPortals(int startIdx, int endIdx, float[] endPos, dtPoly[] path,
								  float[] straightPath, char[] straightPathFlags, dtPoly[] straightPathRefs,
								  int[] straightPathCount, int maxStraightPath, int options)
	{
		float[] startPos = straightPath;//[(straightPathCount[0]-1)*3];
		int startPosIndex = (straightPathCount[0] - 1) * 3;
		// Append or update last vertex
		dtStatus stat = null;
		for (int i = startIdx; i < endIdx; i++)
		{
			// Calculate portal
			dtPoly from = path[i];
			dtMeshTile[] fromTile = new dtMeshTile[1];
			dtPoly[] fromPoly = new dtPoly[1];
			if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(from, fromTile, fromPoly)))
				return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

			dtPoly to = path[i + 1];
			dtMeshTile[] toTile = new dtMeshTile[1];
			dtPoly[] toPoly = new dtPoly[1];
			if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(to, toTile, toPoly)))
				return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

			float left[] = new float[3], right[] = new float[3];
			if (dtStatus.dtStatusFailed(getPortalPoints(from, fromPoly[0], fromTile[0], to, toPoly[0], toTile[0], left, right)))
				break;

			if ((options & dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS) != 0)
			{
				// Skip intersection if only area crossings are requested.
				if (fromPoly[0].getArea() == toPoly[0].getArea())
					continue;
			}

			// Append intersection
			float[] s = new float[1];
			float[] t = new float[1];
			if (new DetourCommonImpl().dtIntersectSegSeg2D(startPos, startPosIndex, endPos, left, right, s, t))
			{
				float pt[] = new float[3];
				DetourCommon.dtVlerp(pt, left, 0, right, 0, t[0]);

				stat = appendVertex(pt, (char)0, path[i + 1],
									straightPath, straightPathFlags, straightPathRefs,
									straightPathCount, maxStraightPath);
				if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
					return stat;
			}
		}
		return new dtStatus(dtStatus.DT_IN_PROGRESS);
	}

	/// @par
	///
	/// This method peforms what is often called 'string pulling'.
	///
	/// The start position is clamped to the first polygon in the path, and the
	/// end position is clamped to the last. So the start and end positions should
	/// normally be within or very near the first and last polygons respectively.
	///
	/// The returned polygon references represent the reference id of the polygon
	/// that is entered at the associated path position. The reference id associated
	/// with the end point will always be zero.  This allows, for example, matching
	/// off-mesh link points to their representative polygons.
	///
	/// If the provided result buffers are too small for the entire result set,
	/// they will be filled as far as possible from the start toward the end
	/// position.
	///
	public dtStatus findStraightPath(float[] startPos, float[] endPos,
									 dtPoly[] path, int pathSize,
									 float[] straightPath, char[] straightPathFlags, dtPoly[] straightPathRefs,
									 int[] straightPathCount, int maxStraightPath, int options)
	{
//		dtAssert(m_nav);

		straightPathCount[0] = 0;

		if (maxStraightPath == 0)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		if (path[0] == null)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		dtStatus stat = new dtStatus(0);

		// TODO: Should this be callers responsibility?
		float closestStartPos[] = new float[3];
		if (dtStatus.dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		float closestEndPos[] = new float[3];
		if (dtStatus.dtStatusFailed(closestPointOnPolyBoundary(path[pathSize - 1], endPos, closestEndPos)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		// Add start point.
		stat = appendVertex(closestStartPos, (char)dtStraightPathFlags.DT_STRAIGHTPATH_START, path[0],
							straightPath, straightPathFlags, straightPathRefs,
							straightPathCount, maxStraightPath);
		if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
			return stat;

		if (pathSize > 1)
		{
			float portalApex[] = new float[3], portalLeft[] = new float[3], portalRight[] = new float[3];
			DetourCommon.dtVcopy(portalApex, closestStartPos);
			DetourCommon.dtVcopy(portalLeft, portalApex);
			DetourCommon.dtVcopy(portalRight, portalApex);
			int apexIndex = 0;
			int leftIndex = 0;
			int rightIndex = 0;

			char leftPolyType = 0;
			char rightPolyType = 0;

			dtPoly leftPolyRef = path[0];
			dtPoly rightPolyRef = path[0];

			for (int i = 0; i < pathSize; ++i)
			{
				float left[] = new float[3], right[] = new float[3];
				char[] fromType = new char[1];
				char toType[] = new char[1];

				if (i + 1 < pathSize)
				{
					// Next portal.
					if (dtStatus.dtStatusFailed(getPortalPoints(path[i], path[i + 1], left, right, fromType, toType)))
					{
						// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
						// Clamp the end point to path[i], and return the path so far.

						if (dtStatus.dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
						{
							// This should only happen when the first polygon is invalid.
							return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
						}

						// Apeend portals along the current straight path segment.
						if ((options & (dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
						{
							stat = appendPortals(apexIndex, i, closestEndPos, path,
												 straightPath, straightPathFlags, straightPathRefs,
												 straightPathCount, maxStraightPath, options);
						}

						stat = appendVertex(closestEndPos, (char)0, path[i],
											straightPath, straightPathFlags, straightPathRefs,
											straightPathCount, maxStraightPath);

						return new dtStatus(dtStatus.DT_SUCCESS | dtStatus.DT_PARTIAL_RESULT | ((straightPathCount[0] >= maxStraightPath) ? dtStatus.DT_BUFFER_TOO_SMALL : 0));
					}

					// If starting really close the portal, advance.
					if (i == 0)
					{
						float[] t = new float[1];
						if (new DetourCommonImpl().dtDistancePtSegSqr2D(portalApex, left, right, t) < DetourCommon.dtSqr(0.001f))
							continue;
					}
				}
				else
				{
					// End of the path.
					DetourCommon.dtVcopy(left, closestEndPos);
					DetourCommon.dtVcopy(right, closestEndPos);

					fromType[0] = toType[0] = (char)dtPolyTypes.DT_POLYTYPE_GROUND;
				}

				// Right vertex.
				if (DetourCommon.dtTriArea2D(portalApex, portalRight, right) <= 0.0f)
				{
					if (DetourCommon.dtVequal(portalApex, portalRight) || DetourCommon.dtTriArea2D(portalApex, portalLeft, right) > 0.0f)
					{
						DetourCommon.dtVcopy(portalRight, right);
						rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : null;
						rightPolyType = toType[0];
						rightIndex = i;
					}
					else
					{
						// Append portals along the current straight path segment.
						if ((options & (dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
						{
							stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
												 straightPath, straightPathFlags, straightPathRefs,
												 straightPathCount, maxStraightPath, options);
							if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
								return stat;
						}

						DetourCommon.dtVcopy(portalApex, portalLeft);
						apexIndex = leftIndex;

						char flags = 0;
						if (leftPolyRef == null)
							flags = dtStraightPathFlags.DT_STRAIGHTPATH_END;
						else if (leftPolyType == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							flags = dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION;
						dtPoly ref = leftPolyRef;

						// Append or update vertex
						stat = appendVertex(portalApex, flags, ref,
											straightPath, straightPathFlags, straightPathRefs,
											straightPathCount, maxStraightPath);
						if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
							return stat;

						DetourCommon.dtVcopy(portalLeft, portalApex);
						DetourCommon.dtVcopy(portalRight, portalApex);
						leftIndex = apexIndex;
						rightIndex = apexIndex;

						// Restart
						i = apexIndex;

						continue;
					}
				}

				// Left vertex.
				if (DetourCommon.dtTriArea2D(portalApex, portalLeft, left) >= 0.0f)
				{
					if (DetourCommon.dtVequal(portalApex, portalLeft) || DetourCommon.dtTriArea2D(portalApex, portalRight, left) < 0.0f)
					{
						DetourCommon.dtVcopy(portalLeft, left);
						leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : null;
						leftPolyType = toType[0];
						leftIndex = i;
					}
					else
					{
						// Append portals along the current straight path segment.
						if ((options & (dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
						{
							stat = appendPortals(apexIndex, rightIndex, portalRight, path,
												 straightPath, straightPathFlags, straightPathRefs,
												 straightPathCount, maxStraightPath, options);
							if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
								return stat;
						}

						DetourCommon.dtVcopy(portalApex, portalRight);
						apexIndex = rightIndex;

						char flags = 0;
						if (rightPolyRef == null)
							flags = dtStraightPathFlags.DT_STRAIGHTPATH_END;
						else if (rightPolyType == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
							flags = dtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION;
						dtPoly ref = rightPolyRef;

						// Append or update vertex
						stat = appendVertex(portalApex, flags, ref,
											straightPath, straightPathFlags, straightPathRefs,
											straightPathCount, maxStraightPath);
						if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
							return stat;

						DetourCommon.dtVcopy(portalLeft, portalApex);
						DetourCommon.dtVcopy(portalRight, portalApex);
						leftIndex = apexIndex;
						rightIndex = apexIndex;

						// Restart
						i = apexIndex;

						continue;
					}
				}
			}

			// Append portals along the current straight path segment.
			if ((options & (dtStraightPathOptions.DT_STRAIGHTPATH_AREA_CROSSINGS | dtStraightPathOptions.DT_STRAIGHTPATH_ALL_CROSSINGS)) != 0)
			{
				stat = appendPortals(apexIndex, pathSize - 1, closestEndPos, path,
									 straightPath, straightPathFlags, straightPathRefs,
									 straightPathCount, maxStraightPath, options);
				if (stat.dtStatus != dtStatus.DT_IN_PROGRESS)
					return stat;
			}
		}

		stat = appendVertex(closestEndPos, (char)dtStraightPathFlags.DT_STRAIGHTPATH_END, null,
							straightPath, straightPathFlags, straightPathRefs,
							straightPathCount, maxStraightPath);

		return new dtStatus(dtStatus.DT_SUCCESS | ((straightPathCount[0] >= maxStraightPath) ? dtStatus.DT_BUFFER_TOO_SMALL : 0));
	}

	/// @par
	///
	/// This method is optimized for small delta movement and a small number of
	/// polygons. If used for too great a distance, the result set will form an
	/// incomplete path.
	///
	/// @p resultPos will equal the @p endPos if the end is reached.
	/// Otherwise the closest reachable position will be returned.
	///
	/// @p resultPos is not projected onto the surface of the navigation
	/// mesh. Use #getPolyHeight if this is needed.
	///
	/// This method treats the end position in the same manner as
	/// the #raycast method. (As a 2D point.) See that method's documentation
	/// for details.
	///
	/// If the @p visited array is too small to hold the entire result set, it will
	/// be filled as far as possible from the start position toward the end
	/// position.
	///
	public dtStatus moveAlongSurface(dtPoly startRef, float[] startPos, float[] endPos,
									 dtQueryFilter filter,
									 float[] resultPos, dtPoly[] visited, int[] visitedCount, int maxVisitedSize)
	{
//		dtAssert(m_nav);
//		dtAssert(m_tinyNodePool);

		visitedCount[0] = 0;

		// Validate input
		if (startRef == null)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		if (!m_nav.isValidPolyRef(startRef))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		dtStatus status = new dtStatus(dtStatus.DT_SUCCESS);

		int MAX_STACK = 48;
		dtNode stack[] = new dtNode[MAX_STACK];
		int nstack = 0;

		m_tinyNodePool.clear();

		dtNode startNode = m_tinyNodePool.getNode(startRef);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = 0;
		startNode.id = startRef;
		startNode.flags = dtNodeFlags.DT_NODE_CLOSED;
		stack[nstack++] = startNode;

		float bestPos[] = new float[3];
		float bestDist = Float.MAX_VALUE;
		dtNode bestNode = null;
		DetourCommon.dtVcopy(bestPos, startPos);

		// Search raints
		float searchPos[] = new float[3], searchRadSqr;
		DetourCommon.dtVlerp(searchPos, startPos, 0, endPos, 0, 0.5f);
		searchRadSqr = DetourCommon.dtSqr(DetourCommon.dtVdist(startPos, endPos) / 2.0f + 0.001f);

		float verts[] = new float[DetourNavMesh.DT_VERTS_PER_POLYGON * 3];

		while (nstack != 0)
		{
			// Pop front.
			dtNode curNode = stack[0];
			for (int i = 0; i < nstack - 1; ++i)
				stack[i] = stack[i + 1];
			nstack--;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			dtPoly curRef = curNode.id;
			dtMeshTile[] curTile = new dtMeshTile[1];
			dtPoly curPoly[] = new dtPoly[1];
			m_nav.getTileAndPolyByRefUnsafe(curRef, curTile, curPoly);

			// Collect vertices.
			int nverts = curPoly[0].vertCount;
			for (int i = 0; i < nverts; ++i)
				DetourCommon.dtVcopy(verts, i * 3, curTile[0].verts, curPoly[0].verts[i] * 3);

			// If target is inside the poly, stop search.
			if (new DetourCommonImpl().dtPointInPolygon(endPos, verts, nverts))
			{
				bestNode = curNode;
				DetourCommon.dtVcopy(bestPos, endPos);
				break;
			}

			// Find wall edges and find nearest point inside the walls.
			for (int i = 0, j = (int)curPoly[0].vertCount - 1; i < (int)curPoly[0].vertCount; j = i++)
			{
				// Find links to neighbours.
				int MAX_NEIS = 8;
				int nneis = 0;
				dtPoly[] neis = new dtPoly[MAX_NEIS];

				if ((curPoly[0].neis[j] & DetourNavMesh.DT_EXT_LINK) != 0)
				{
					// Tile border.
					for (int k = curPoly[0].firstLink; k != DetourNavMesh.DT_NULL_LINK; k = curTile[0].links[k].next)
					{
						dtLink link = curTile[0].links[k];
						if (link.edge == j)
						{
							if (link.ref != null)
							{
								dtMeshTile[] neiTile = new dtMeshTile[1];
								dtPoly[] neiPoly = new dtPoly[1];
								m_nav.getTileAndPolyByRefUnsafe(link.ref, neiTile, neiPoly);
								if (filter.passFilter(link.ref, neiTile, neiPoly))
								{
									if (nneis < MAX_NEIS)
										neis[nneis++] = link.ref;
								}
							}
						}
					}
				}
				else if (curPoly[0].neis[j] != 0)
				{
					//todo [IZ]
//					 int idx = (int)(curPoly[0].neis[j]-1);
//					 dtPoly ref = m_nav.getPolyRefBase(curTile[0]) | idx;
//					if (filter.passFilter(ref, curTile[0], curTile[0].polys[idx]))
//					{
//						// Internal edge, encode id.
//						neis[nneis++] = ref;
//					}
				}

				if (nneis == 0)
				{
					// Wall edge, calc distance.
//					 float* vj = &verts[j*3];
//					 float* vi = &verts[i*3];
					float[] tseg = new float[1];
					float distSqr = new DetourCommonImpl().dtDistancePtSegSqr2D(endPos, verts, j * 3, verts, i * 3, tseg, 0);
					if (distSqr < bestDist)
					{
						// Update nearest distance.
						DetourCommon.dtVlerp(bestPos, verts, j * 3, verts, i * 3, tseg[0]);
						bestDist = distSqr;
						bestNode = curNode;
					}
				}
				else
				{
					for (int k = 0; k < nneis; ++k)
					{
						// Skip if no node can be allocated.
						dtNode neighbourNode = m_tinyNodePool.getNode(neis[k]);
						if (neighbourNode == null)
							continue;
						// Skip if already visited.
						if ((neighbourNode.flags & dtNodeFlags.DT_NODE_CLOSED) != 0)
							continue;

						// Skip the link if it is too far from search raint.
						// TODO: Maybe should use getPortalPoints(), but this one is way faster.
//						 float* vj = &verts[j*3];
//						 float* vi = &verts[i*3];
						float[] tseg = new float[3];
						float distSqr = new DetourCommonImpl().dtDistancePtSegSqr2D(searchPos, verts, j * 3, verts, i * 3, tseg, 0);
						if (distSqr > searchRadSqr)
							continue;

						// Mark as the node as visited and push to queue.
						if (nstack < MAX_STACK)
						{
							neighbourNode.pidx = m_tinyNodePool.getNodeIdx(curNode);
							neighbourNode.flags |= dtNodeFlags.DT_NODE_CLOSED;
							stack[nstack++] = neighbourNode;
						}
					}
				}
			}
		}

		int n = 0;
		if (bestNode != null)
		{
			// Reverse the path.
			dtNode prev = null;
			dtNode node = bestNode;
			do
			{
				dtNode next = m_tinyNodePool.getNodeAtIdx(node.pidx);
				node.pidx = m_tinyNodePool.getNodeIdx(prev);
				prev = node;
				node = next;
			}
			while (node != null);

			// Store result
			node = prev;
			do
			{
				visited[n++] = node.id;
				if (n >= maxVisitedSize)
				{
					status.dtStatus |= dtStatus.DT_BUFFER_TOO_SMALL;
					break;
				}
				node = m_tinyNodePool.getNodeAtIdx(node.pidx);
			}
			while (node != null);
		}

		DetourCommon.dtVcopy(resultPos, bestPos);

		visitedCount[0] = n;

		return status;
	}

	public dtStatus getPortalPoints(dtPoly from, dtPoly to, float[] left, float[] right,
									char[] fromType, char[] toType)
	{
//		dtAssert(m_nav);

		dtMeshTile fromTile[] = new dtMeshTile[1];
		dtPoly fromPoly[] = new dtPoly[1];
		if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(from, fromTile, fromPoly)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		fromType[0] = (char)fromPoly[0].getType();

		dtMeshTile[] toTile = new dtMeshTile[1];
		dtPoly[] toPoly = new dtPoly[1];
		if (dtStatus.dtStatusFailed(m_nav.getTileAndPolyByRef(to, toTile, toPoly)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		toType[0] = (char)toPoly[0].getType();

		return getPortalPoints(from, fromPoly[0], fromTile[0], to, toPoly[0], toTile[0], left, right);
	}

	// Returns portal points between two polygons.
	public dtStatus getPortalPoints(dtPoly from, dtPoly fromPoly, dtMeshTile fromTile,
									dtPoly to, dtPoly toPoly, dtMeshTile toTile,
									float[] left, float[] right)
	{
		// Find the link that points to the 'to' polygon.
		dtLink link = null;
		for (int i = fromPoly.firstLink; i != DetourNavMesh.DT_NULL_LINK; i = fromTile.links[i].next)
		{
			if (fromTile.links[i].ref == to)
			{
				link = fromTile.links[i];
				break;
			}
		}
		if (link == null)
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		// Handle off-mesh connections.
		if (fromPoly.getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
		{
			// Find link that points to first vertex.
			for (int i = fromPoly.firstLink; i != DetourNavMesh.DT_NULL_LINK; i = fromTile.links[i].next)
			{
				if (fromTile.links[i].ref == to)
				{
					int v = fromTile.links[i].edge;
					DetourCommon.dtVcopy(left, 0, fromTile.verts, fromPoly.verts[v] * 3);
					DetourCommon.dtVcopy(right, 0, fromTile.verts, fromPoly.verts[v] * 3);
					return new dtStatus(dtStatus.DT_SUCCESS);
				}
			}
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		}

		if (toPoly.getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
		{
			for (int i = toPoly.firstLink; i != DetourNavMesh.DT_NULL_LINK; i = toTile.links[i].next)
			{
				if (toTile.links[i].ref == from)
				{
					int v = toTile.links[i].edge;
					DetourCommon.dtVcopy(left, 0, toTile.verts, toPoly.verts[v] * 3);
					DetourCommon.dtVcopy(right, 0, toTile.verts, toPoly.verts[v] * 3);
					return new dtStatus(dtStatus.DT_SUCCESS);
				}
			}
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		}

		// Find portal vertices.
		int v0 = fromPoly.verts[link.edge];
		int v1 = fromPoly.verts[(link.edge + 1) % (int)fromPoly.vertCount];
		DetourCommon.dtVcopy(left, 0, fromTile.verts, v0 * 3);
		DetourCommon.dtVcopy(right, 0, fromTile.verts, v1 * 3);

		// If the link is at tile boundary, dtClamp the vertices to
		// the link width.
		if (link.side != 0xff)
		{
			// Unpack portal limits.
			if (link.bmin != 0 || link.bmax != 255)
			{
				float s = 1.0f / 255.0f;
				float tmin = link.bmin * s;
				float tmax = link.bmax * s;
				DetourCommon.dtVlerp(left, fromTile.verts, v0 * 3, fromTile.verts, v1 * 3, tmin);
				DetourCommon.dtVlerp(right, fromTile.verts, v0 * 3, fromTile.verts, v1 * 3, tmax);
			}
		}

		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	// Returns edge mid point between two polygons.
	public dtStatus getEdgeMidPoint(dtPoly from, dtPoly to, float[] mid)
	{
		float left[] = new float[3], right[] = new float[3];
		char fromType[] = new char[1], toType[] = new char[1];
		if (dtStatus.dtStatusFailed(getPortalPoints(from, to, left, right, fromType, toType)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		mid[0] = (left[0] + right[0]) * 0.5f;
		mid[1] = (left[1] + right[1]) * 0.5f;
		mid[2] = (left[2] + right[2]) * 0.5f;
		return new dtStatus(dtStatus.DT_SUCCESS);
	}

	/*dtStatus getEdgeMidPoint(dtPoly from,  dtPoly* fromPoly,  dtMeshTile* fromTile,
											 dtPoly to,  dtPoly* toPoly,  dtMeshTile* toTile,
											 float* mid) 
	{
		float left[3], right[3];
		if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
			return DT_FAILURE | DT_INVALID_PARAM;
		mid[0] = (left[0]+right[0])*0.5f;
		mid[1] = (left[1]+right[1])*0.5f;
		mid[2] = (left[2]+right[2])*0.5f;
		return DT_SUCCESS;
	} */

	/// @par
	///
	/// This method is meant to be used for quick, short distance checks.
	///
	/// If the path array is too small to hold the result, it will be filled as
	/// far as possible from the start postion toward the end position.
	///
	/// <b>Using the Hit Parameter (t)</b>
	///
	/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
	/// the end position. In this case the path represents a valid corridor to the
	/// end position and the value of @p hitNormal is undefined.
	///
	/// If the hit parameter is zero, then the start position is on the wall that
	/// was hit and the value of @p hitNormal is undefined.
	///
	/// If 0 < t < 1.0 then the following applies:
	///
	/// @code
	/// distanceToHitBorder = distanceToEndPosition * t
	/// hitPoint = startPos + (endPos - startPos) * t
	/// @endcode
	///
	/// <b>Use Case Restriction</b>
	///
	/// The raycast ignores the y-value of the end position. (2D check.) This
	/// places significant limits on how it can be used. For example:
	///
	/// Consider a scene where there is a main floor with a second floor balcony
	/// that hangs over the main floor. So the first floor mesh extends below the
	/// balcony mesh. The start position is somewhere on the first floor. The end
	/// position is on the balcony.
	///
	/// The raycast will search toward the end position along the first floor mesh.
	/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
	/// (no wall hit), meaning it reached the end position. This is one example of why
	/// this method is meant for short distance checks.
	///
	public dtStatus raycast(dtPoly startRef, float[] startPos, float[] endPos,
							dtQueryFilter filter,
							float[] t, float[] hitNormal, dtPoly[] path, int[] pathCount, int maxPath)
	{
//		dtAssert(m_nav);

		t[0] = 0;
		if (pathCount != null)
			pathCount[0] = 0;

		// Validate input
		if (startRef == null || !m_nav.isValidPolyRef(startRef))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);

		dtPoly curRef = startRef;
		float verts[] = new float[DetourNavMesh.DT_VERTS_PER_POLYGON * 3];
		int n = 0;

		hitNormal[0] = 0;
		hitNormal[1] = 0;
		hitNormal[2] = 0;

		dtStatus status = new dtStatus(dtStatus.DT_SUCCESS);

		while (curRef != null)
		{
			// Cast ray against current polygon.

			// The API input has been cheked already, skip checking internal data.
			dtMeshTile tile[] = new dtMeshTile[1];
			dtPoly poly[] = new dtPoly[1];
			m_nav.getTileAndPolyByRefUnsafe(curRef, tile, poly);

			// Collect vertices.
			int nv = 0;
			for (int i = 0; i < (int)poly[0].vertCount; ++i)
			{
				DetourCommon.dtVcopy(verts, nv * 3, tile[0].verts, poly[0].verts[i] * 3);
				nv++;
			}

			float[] tmin = new float[1];
			float tmax[] = new float[1];
			int segMin[] = new int[1], segMax[] = new int[1];
			if (!new DetourCommonImpl().dtIntersectSegmentPoly2D(startPos, endPos, verts, nv, tmin, tmax, segMin, segMax))
			{
				// Could not hit the polygon, keep the old t and report hit.
				if (pathCount != null)
					pathCount[0] = n;
				return status;
			}
			// Keep track of furthest t so far.
			if (tmax[0] > t[0])
				t[0] = tmax[0];

			// Store visited polygons.
			if (n < maxPath)
				path[n++] = curRef;
			else
				status.dtStatus |= dtStatus.DT_BUFFER_TOO_SMALL;

			// Ray end is completely inside the polygon.
			if (segMax[0] == -1)
			{
				t[0] = Float.MAX_VALUE;
				if (pathCount != null)
					pathCount[0] = n;
				return status;
			}

			// Follow neighbours.
			dtPoly nextRef = null;

			for (int i = poly[0].firstLink; i != DetourNavMesh.DT_NULL_LINK; i = tile[0].links[i].next)
			{
				dtLink link = tile[0].links[i];

				// Find link which contains this edge.
				if ((int)link.edge != segMax[0])
					continue;

				// Get pointer to the next polygon.
				dtMeshTile[] nextTile = new dtMeshTile[1];
				dtPoly[] nextPoly = new dtPoly[1];
				m_nav.getTileAndPolyByRefUnsafe(link.ref, nextTile, nextPoly);

				// Skip off-mesh connections.
				if (nextPoly[0].getType() == dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
					continue;

				// Skip links based on filter.
				if (!filter.passFilter(link.ref, nextTile, nextPoly))
					continue;

				// If the link is internal, just return the ref.
				if (link.side == 0xff)
				{
					nextRef = link.ref;
					break;
				}

				// If the link is at tile boundary,

				// Check if the link spans the whole edge, and accept.
				if (link.bmin == 0 && link.bmax == 255)
				{
					nextRef = link.ref;
					break;
				}

				// Check for partial edge links.
				int v0 = poly[0].verts[link.edge];
				int v1 = poly[0].verts[(link.edge + 1) % poly[0].vertCount];
				float[] left = tile[0].verts;
				int leftIndex = v0 * 3;
				float[] right = tile[0].verts;
				int rightIndex = v1 * 3;

				// Check that the intersection lies inside the link portal.
				if (link.side == 0 || link.side == 4)
				{
					// Calculate link size.
					float s = 1.0f / 255.0f;
					float lmin = left[leftIndex + 2] + (right[rightIndex + 2] - left[leftIndex + 2]) * (link.bmin * s);
					float lmax = left[leftIndex + 2] + (right[rightIndex + 2] - left[leftIndex + 2]) * (link.bmax * s);
					if (lmin > lmax)
					{
						float tmp = lmax;
						lmax = lmin;
						lmin = tmp;
					}
//						DetourCommon.dtSwap(lmin, lmax);

					// Find Z intersection.
					float z = startPos[2] + (endPos[2] - startPos[2]) * tmax[0];
					if (z >= lmin && z <= lmax)
					{
						nextRef = link.ref;
						break;
					}
				}
				else if (link.side == 2 || link.side == 6)
				{
					// Calculate link size.
					float s = 1.0f / 255.0f;
					float lmin = left[leftIndex + 0] + (right[rightIndex + 0] - left[leftIndex + 0]) * (link.bmin * s);
					float lmax = left[leftIndex + 0] + (right[rightIndex + 0] - left[leftIndex + 0]) * (link.bmax * s);
					if (lmin > lmax)
					{
						float tmp = lmax;
						lmax = lmin;
						lmin = tmp;
//						DetourCommon.dtSwap(lmin, lmax);
					}

					// Find X intersection.
					float x = startPos[0] + (endPos[0] - startPos[0]) * tmax[0];
					if (x >= lmin && x <= lmax)
					{
						nextRef = link.ref;
						break;
					}
				}
			}

			if (nextRef == null)
			{
				// No neighbour, we hit a wall.

				// Calculate hit normal.
				int a = segMax[0];
				int b = segMax[0] + 1 < nv ? segMax[0] + 1 : 0;
				float[] va = verts;//[a*3];
				int vaIndex = a * 3;
				float[] vb = verts;//[b*3];
				int vbIndex = b * 3;
				float dx = vb[vbIndex + 0] - va[vaIndex + 0];
				float dz = vb[vbIndex + 2] - va[vaIndex + 2];
				hitNormal[0] = dz;
				hitNormal[1] = 0;
				hitNormal[2] = -dx;
				DetourCommon.dtVnormalize(hitNormal);

				if (pathCount != null)
					pathCount[0] = n;
				return status;
			}

			// No hit, advance to neighbour polygon.
			curRef = nextRef;
		}

		if (pathCount != null)
			pathCount[0] = n;

		return status;
	}

	/// @par
	///
	/// At least one result array must be provided.
	///
	/// The order of the result set is from least to highest cost to reach the polygon.
	///
	/// A common use case for this method is to perform Dijkstra searches.
	/// Candidate polygons are found by searching the graph beginning at the start polygon.
	///
	/// If a polygon is not found via the graph search, even if it intersects the
	/// search circle, it will not be included in the result set. For example:
	///
	/// polyA is the start polygon.
	/// polyB shares an edge with polyA. (Is adjacent.)
	/// polyC shares an edge with polyB, but not with polyA
	/// Even if the search circle overlaps polyC, it will not be included in the
	/// result set unless polyB is also in the set.
	///
	/// The value of the center point is used as the start position for cost
	/// calculations. It is not projected onto the surface of the mesh, so its
	/// y-value will effect the costs.
	///
	/// Intersection tests occur in 2D. All polygons and the search circle are
	/// projected onto the xz-plane. So the y-value of the center point does not
	/// effect intersection tests.
	///
	/// If the result arrays are to small to hold the entire result set, they will be
	/// filled to capacity.
	///
/*	dtStatus findPolysAroundCircle(dtPoly startRef,  float* centerPos,  float radius,
												    dtQueryFilter* filter,
												   dtPoly* resultRef, dtPoly* resultParent, float* resultCost,
												   int* resultCount,  int maxResult) 
	{
		dtAssert(m_nav);
		dtAssert(m_nodePool);
		dtAssert(m_openList);

		*resultCount = 0;

		// Validate input
		if (!startRef || !m_nav.isValidPolyRef(startRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		m_nodePool.clear();
		m_openList.clear();

		dtNode* startNode = m_nodePool.getNode(startRef);
		dtVcopy(startNode.pos, centerPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = 0;
		startNode.id = startRef;
		startNode.flags = DT_NODE_OPEN;
		m_openList.push(startNode);

		dtStatus status = DT_SUCCESS;

		int n = 0;
		if (n < maxResult)
		{
			if (resultRef)
				resultRef[n] = startNode.id;
			if (resultParent)
				resultParent[n] = 0;
			if (resultCost)
				resultCost[n] = 0;
			++n;
		}
		else
		{
			status |= DT_BUFFER_TOO_SMALL;
		}

		 float radiusSqr = dtSqr(radius);

		while (!m_openList.empty())
		{
			dtNode* bestNode = m_openList.pop();
			bestNode.flags &= ~DT_NODE_OPEN;
			bestNode.flags |= DT_NODE_CLOSED;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly bestRef = bestNode.id;
			 dtMeshTile* bestTile = 0;
			 dtPoly* bestPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

			// Get parent poly and tile.
			dtPoly parentRef = 0;
			 dtMeshTile* parentTile = 0;
			 dtPoly* parentPoly = 0;
			if (bestNode.pidx)
				parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef)
				m_nav.getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

			for (unsigned int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
			{
				 dtLink* link = &bestTile.links[i];
				dtPoly neighbourRef = link.ref;
				// Skip invalid neighbours and do not follow back to parent.
				if (!neighbourRef || neighbourRef == parentRef)
					continue;

				// Expand to neighbour
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				// Do not advance if the polygon is excluded by the filter.
				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				// Find edge and calc distance to the edge.
				float va[3], vb[3];
				if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
					continue;

				// If the circle is not touching the next polygon, skip it.
				float tseg;
				float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
				if (distSqr > radiusSqr)
					continue;

				dtNode* neighbourNode = m_nodePool.getNode(neighbourRef);
				if (!neighbourNode)
				{
					status |= DT_OUT_OF_NODES;
					continue;
				}

				if (neighbourNode.flags & DT_NODE_CLOSED)
					continue;

				// Cost
				if (neighbourNode.flags == 0)
					dtVlerp(neighbourNode.pos, va, vb, 0.5f);

				 float total = bestNode.total + dtVdist(bestNode.pos, neighbourNode.pos);

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_OPEN) && total >= neighbourNode.total)
					continue;

				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.total = total;

				if (neighbourNode.flags & DT_NODE_OPEN)
				{
					m_openList.modify(neighbourNode);
				}
				else
				{
					if (n < maxResult)
					{
						if (resultRef)
							resultRef[n] = neighbourNode.id;
						if (resultParent)
							resultParent[n] = m_nodePool.getNodeAtIdx(neighbourNode.pidx).id;
						if (resultCost)
							resultCost[n] = neighbourNode.total;
						++n;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
					neighbourNode.flags = DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}
			}
		}

		*resultCount = n;

		return status;
	}

	/// @par
	///
	/// The order of the result set is from least to highest cost.
	///
	/// At least one result array must be provided.
	///
	/// A common use case for this method is to perform Dijkstra searches.
	/// Candidate polygons are found by searching the graph beginning at the start
	/// polygon.
	///
	/// The same intersection test restrictions that apply to findPolysAroundCircle()
	/// method apply to this method.
	///
	/// The 3D centroid of the search polygon is used as the start position for cost
	/// calculations.
	///
	/// Intersection tests occur in 2D. All polygons are projected onto the
	/// xz-plane. So the y-values of the vertices do not effect intersection tests.
	///
	/// If the result arrays are is too small to hold the entire result set, they will
	/// be filled to capacity.
	///
	dtStatus findPolysAroundShape(dtPoly startRef,  float* verts,  int nverts,
												   dtQueryFilter* filter,
												  dtPoly* resultRef, dtPoly* resultParent, float* resultCost,
												  int* resultCount,  int maxResult) 
	{
		dtAssert(m_nav);
		dtAssert(m_nodePool);
		dtAssert(m_openList);

		*resultCount = 0;

		// Validate input
		if (!startRef || !m_nav.isValidPolyRef(startRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		m_nodePool.clear();
		m_openList.clear();

		float centerPos[3] = {0,0,0};
		for (int i = 0; i < nverts; ++i)
			dtVadd(centerPos,centerPos,&verts[i*3]);
		dtVscale(centerPos,centerPos,1.0f/nverts);

		dtNode* startNode = m_nodePool.getNode(startRef);
		dtVcopy(startNode.pos, centerPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = 0;
		startNode.id = startRef;
		startNode.flags = DT_NODE_OPEN;
		m_openList.push(startNode);

		dtStatus status = DT_SUCCESS;

		int n = 0;
		if (n < maxResult)
		{
			if (resultRef)
				resultRef[n] = startNode.id;
			if (resultParent)
				resultParent[n] = 0;
			if (resultCost)
				resultCost[n] = 0;
			++n;
		}
		else
		{
			status |= DT_BUFFER_TOO_SMALL;
		}

		while (!m_openList.empty())
		{
			dtNode* bestNode = m_openList.pop();
			bestNode.flags &= ~DT_NODE_OPEN;
			bestNode.flags |= DT_NODE_CLOSED;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly bestRef = bestNode.id;
			 dtMeshTile* bestTile = 0;
			 dtPoly* bestPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

			// Get parent poly and tile.
			dtPoly parentRef = 0;
			 dtMeshTile* parentTile = 0;
			 dtPoly* parentPoly = 0;
			if (bestNode.pidx)
				parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef)
				m_nav.getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

			for (unsigned int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
			{
				 dtLink* link = &bestTile.links[i];
				dtPoly neighbourRef = link.ref;
				// Skip invalid neighbours and do not follow back to parent.
				if (!neighbourRef || neighbourRef == parentRef)
					continue;

				// Expand to neighbour
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				// Do not advance if the polygon is excluded by the filter.
				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				// Find edge and calc distance to the edge.
				float va[3], vb[3];
				if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
					continue;

				// If the poly is not touching the edge to the next polygon, skip the connection it.
				float tmin, tmax;
				int segMin, segMax;
				if (!dtIntersectSegmentPoly2D(va, vb, verts, nverts, tmin, tmax, segMin, segMax))
					continue;
				if (tmin > 1.0f || tmax < 0.0f)
					continue;

				dtNode* neighbourNode = m_nodePool.getNode(neighbourRef);
				if (!neighbourNode)
				{
					status |= DT_OUT_OF_NODES;
					continue;
				}

				if (neighbourNode.flags & DT_NODE_CLOSED)
					continue;

				// Cost
				if (neighbourNode.flags == 0)
					dtVlerp(neighbourNode.pos, va, vb, 0.5f);

				 float total = bestNode.total + dtVdist(bestNode.pos, neighbourNode.pos);

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_OPEN) && total >= neighbourNode.total)
					continue;

				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.total = total;

				if (neighbourNode.flags & DT_NODE_OPEN)
				{
					m_openList.modify(neighbourNode);
				}
				else
				{
					if (n < maxResult)
					{
						if (resultRef)
							resultRef[n] = neighbourNode.id;
						if (resultParent)
							resultParent[n] = m_nodePool.getNodeAtIdx(neighbourNode.pidx).id;
						if (resultCost)
							resultCost[n] = neighbourNode.total;
						++n;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
					neighbourNode.flags = DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}
			}
		}

		*resultCount = n;

		return status;
	}*/

	/// @par
	///
	/// This method is optimized for a small search radius and small number of result
	/// polygons.
	///
	/// Candidate polygons are found by searching the navigation graph beginning at
	/// the start polygon.
	///
	/// The same intersection test restrictions that apply to the findPolysAroundCircle
	/// mehtod applies to this method.
	///
	/// The value of the center point is used as the start point for cost calculations.
	/// It is not projected onto the surface of the mesh, so its y-value will effect
	/// the costs.
	///
	/// Intersection tests occur in 2D. All polygons and the search circle are
	/// projected onto the xz-plane. So the y-value of the center point does not
	/// effect intersection tests.
	///
	/// If the result arrays are is too small to hold the entire result set, they will
	/// be filled to capacity.
	///
/*	dtStatus findLocalNeighbourhood(dtPoly startRef,  float* centerPos,  float radius,
													 dtQueryFilter* filter,
													dtPoly* resultRef, dtPoly* resultParent,
													int* resultCount,  int maxResult) 
	{
		dtAssert(m_nav);
		dtAssert(m_tinyNodePool);

		*resultCount = 0;

		// Validate input
		if (!startRef || !m_nav.isValidPolyRef(startRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		static  int MAX_STACK = 48;
		dtNode* stack[MAX_STACK];
		int nstack = 0;

		m_tinyNodePool.clear();

		dtNode* startNode = m_tinyNodePool.getNode(startRef);
		startNode.pidx = 0;
		startNode.id = startRef;
		startNode.flags = DT_NODE_CLOSED;
		stack[nstack++] = startNode;

		 float radiusSqr = dtSqr(radius);

		float pa[DT_VERTS_PER_POLYGON*3];
		float pb[DT_VERTS_PER_POLYGON*3];

		dtStatus status = DT_SUCCESS;

		int n = 0;
		if (n < maxResult)
		{
			resultRef[n] = startNode.id;
			if (resultParent)
				resultParent[n] = 0;
			++n;
		}
		else
		{
			status |= DT_BUFFER_TOO_SMALL;
		}

		while (nstack)
		{
			// Pop front.
			dtNode* curNode = stack[0];
			for (int i = 0; i < nstack-1; ++i)
				stack[i] = stack[i+1];
			nstack--;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly curRef = curNode.id;
			 dtMeshTile* curTile = 0;
			 dtPoly* curPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(curRef, &curTile, &curPoly);

			for (unsigned int i = curPoly.firstLink; i != DT_NULL_LINK; i = curTile.links[i].next)
			{
				 dtLink* link = &curTile.links[i];
				dtPoly neighbourRef = link.ref;
				// Skip invalid neighbours.
				if (!neighbourRef)
					continue;

				// Skip if cannot alloca more nodes.
				dtNode* neighbourNode = m_tinyNodePool.getNode(neighbourRef);
				if (!neighbourNode)
					continue;
				// Skip visited.
				if (neighbourNode.flags & DT_NODE_CLOSED)
					continue;

				// Expand to neighbour
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				// Skip off-mesh connections.
				if (neighbourPoly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
					continue;

				// Do not advance if the polygon is excluded by the filter.
				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				// Find edge and calc distance to the edge.
				float va[3], vb[3];
				if (!getPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
					continue;

				// If the circle is not touching the next polygon, skip it.
				float tseg;
				float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
				if (distSqr > radiusSqr)
					continue;

				// Mark node visited, this is done before the overlap test so that
				// we will not visit the poly again if the test fails.
				neighbourNode.flags |= DT_NODE_CLOSED;
				neighbourNode.pidx = m_tinyNodePool.getNodeIdx(curNode);

				// Check that the polygon does not collide with existing polygons.

				// Collect vertices of the neighbour poly.
				 int npa = neighbourPoly.vertCount;
				for (int k = 0; k < npa; ++k)
					dtVcopy(&pa[k*3], &neighbourTile.verts[neighbourPoly.verts[k]*3]);

				bool overlap = false;
				for (int j = 0; j < n; ++j)
				{
					dtPoly pastRef = resultRef[j];

					// Connected polys do not overlap.
					bool connected = false;
					for (unsigned int k = curPoly.firstLink; k != DT_NULL_LINK; k = curTile.links[k].next)
					{
						if (curTile.links[k].ref == pastRef)
						{
							connected = true;
							break;
						}
					}
					if (connected)
						continue;

					// Potentially overlapping.
					 dtMeshTile* pastTile = 0;
					 dtPoly* pastPoly = 0;
					m_nav.getTileAndPolyByRefUnsafe(pastRef, &pastTile, &pastPoly);

					// Get vertices and test overlap
					 int npb = pastPoly.vertCount;
					for (int k = 0; k < npb; ++k)
						dtVcopy(&pb[k*3], &pastTile.verts[pastPoly.verts[k]*3]);

					if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
					{
						overlap = true;
						break;
					}
				}
				if (overlap)
					continue;

				// This poly is fine, store and advance to the poly.
				if (n < maxResult)
				{
					resultRef[n] = neighbourRef;
					if (resultParent)
						resultParent[n] = curRef;
					++n;
				}
				else
				{
					status |= DT_BUFFER_TOO_SMALL;
				}

				if (nstack < MAX_STACK)
				{
					stack[nstack++] = neighbourNode;
				}
			}
		}

		*resultCount = n;

		return status;
	}


	struct dtSegInterval
	{
		dtPoly ref;
		short tmin, tmax;
	};

	static void insertInterval(dtSegInterval* ints, int& nints,  int maxInts,
							    short tmin,  short tmax,  dtPoly ref)
	{
		if (nints+1 > maxInts) return;
		// Find insertion point.
		int idx = 0;
		while (idx < nints)
		{
			if (tmax <= ints[idx].tmin)
				break;
			idx++;
		}
		// Move current results.
		if (nints-idx)
			memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
		// Store
		ints[idx].ref = ref;
		ints[idx].tmin = tmin;
		ints[idx].tmax = tmax;
		nints++;
	}*/

	/// @par
	///
	/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned.
	/// Otherwise only the wall segments are returned.
	///
	/// A segment that is normally a portal will be included in the result set as a
	/// wall if the @p filter results in the neighbor polygon becoomming impassable.
	///
	/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the
	/// maximum segments per polygon of the source navigation mesh.
	///
/*
	dtStatus getPolyWallSegments(dtPoly ref,  dtQueryFilter* filter,
                                                 float* segmentVerts, dtPoly* segmentRefs, int* segmentCount,
                                                  int maxSegments) 
	{
		dtAssert(m_nav);

		*segmentCount = 0;

		 dtMeshTile* tile = 0;
		 dtPoly* poly = 0;
		if (dtStatusFailed(m_nav.getTileAndPolyByRef(ref, &tile, &poly)))
			return DT_FAILURE | DT_INVALID_PARAM;

		int n = 0;
		static  int MAX_INTERVAL = 16;
		dtSegInterval ints[MAX_INTERVAL];
		int nints;

		 bool storePortals = segmentRefs != 0;

		dtStatus status = DT_SUCCESS;

		for (int i = 0, j = (int)poly.vertCount-1; i < (int)poly.vertCount; j = i++)
		{
			// Skip non-solid edges.
			nints = 0;
			if (poly.neis[j] & DT_EXT_LINK)
			{
				// Tile border.
				for (unsigned int k = poly.firstLink; k != DT_NULL_LINK; k = tile.links[k].next)
				{
					 dtLink* link = &tile.links[k];
					if (link.edge == j)
					{
						if (link.ref != 0)
						{
							 dtMeshTile* neiTile = 0;
							 dtPoly* neiPoly = 0;
							m_nav.getTileAndPolyByRefUnsafe(link.ref, &neiTile, &neiPoly);
							if (filter.passFilter(link.ref, neiTile, neiPoly))
							{
								insertInterval(ints, nints, MAX_INTERVAL, link.bmin, link.bmax, link.ref);
							}
						}
					}
				}
			}
			else
			{
				// Internal edge
				dtPoly neiRef = 0;
				if (poly.neis[j])
				{
					 unsigned int idx = (unsigned int)(poly.neis[j]-1);
					neiRef = m_nav.getPolyRefBase(tile) | idx;
					if (!filter.passFilter(neiRef, tile, &tile.polys[idx]))
						neiRef = 0;
				}

				// If the edge leads to another polygon and portals are not stored, skip.
				if (neiRef != 0 && !storePortals)
					continue;

				if (n < maxSegments)
				{
					 float* vj = &tile.verts[poly.verts[j]*3];
					 float* vi = &tile.verts[poly.verts[i]*3];
					float* seg = &segmentVerts[n*6];
					dtVcopy(seg+0, vj);
					dtVcopy(seg+3, vi);
					if (segmentRefs)
						segmentRefs[n] = neiRef;
					n++;
				}
				else
				{
					status |= DT_BUFFER_TOO_SMALL;
				}

				continue;
			}

			// Add sentinels
			insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
			insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);

			// Store segments.
			 float* vj = &tile.verts[poly.verts[j]*3];
			 float* vi = &tile.verts[poly.verts[i]*3];
			for (int k = 1; k < nints; ++k)
			{
				// Portal segment.
				if (storePortals && ints[k].ref)
				{
					 float tmin = ints[k].tmin/255.0f;
					 float tmax = ints[k].tmax/255.0f;
					if (n < maxSegments)
					{
						float* seg = &segmentVerts[n*6];
						dtVlerp(seg+0, vj,vi, tmin);
						dtVlerp(seg+3, vj,vi, tmax);
						if (segmentRefs)
							segmentRefs[n] = ints[k].ref;
						n++;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
				}

				// Wall segment.
				 int imin = ints[k-1].tmax;
				 int imax = ints[k].tmin;
				if (imin != imax)
				{
					 float tmin = imin/255.0f;
					 float tmax = imax/255.0f;
					if (n < maxSegments)
					{
						float* seg = &segmentVerts[n*6];
						dtVlerp(seg+0, vj,vi, tmin);
						dtVlerp(seg+3, vj,vi, tmax);
						if (segmentRefs)
							segmentRefs[n] = 0;
						n++;
					}
					else
					{
						status |= DT_BUFFER_TOO_SMALL;
					}
				}
			}
		}

		*segmentCount = n;

		return status;
	}
*/

	/// @par
	///
	/// @p hitPos is not adjusted using the height detail data.
	///
	/// @p hitDist will equal the search radius if there is no wall within the
	/// radius. In this case the values of @p hitPos and @p hitNormal are
	/// undefined.
	///
	/// The normal will become unpredicable if @p hitDist is a very small number.
	///
	/*dtStatus findDistanceToWall(dtPoly startRef,  float* centerPos,  float maxRadius,
												 dtQueryFilter* filter,
												float* hitDist, float* hitPos, float* hitNormal) 
	{
		dtAssert(m_nav);
		dtAssert(m_nodePool);
		dtAssert(m_openList);

		// Validate input
		if (!startRef || !m_nav.isValidPolyRef(startRef))
			return DT_FAILURE | DT_INVALID_PARAM;

		m_nodePool.clear();
		m_openList.clear();

		dtNode* startNode = m_nodePool.getNode(startRef);
		dtVcopy(startNode.pos, centerPos);
		startNode.pidx = 0;
		startNode.cost = 0;
		startNode.total = 0;
		startNode.id = startRef;
		startNode.flags = DT_NODE_OPEN;
		m_openList.push(startNode);

		float radiusSqr = dtSqr(maxRadius);

		dtStatus status = DT_SUCCESS;

		while (!m_openList.empty())
		{
			dtNode* bestNode = m_openList.pop();
			bestNode.flags &= ~DT_NODE_OPEN;
			bestNode.flags |= DT_NODE_CLOSED;

			// Get poly and tile.
			// The API input has been cheked already, skip checking internal data.
			 dtPoly bestRef = bestNode.id;
			 dtMeshTile* bestTile = 0;
			 dtPoly* bestPoly = 0;
			m_nav.getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);

			// Get parent poly and tile.
			dtPoly parentRef = 0;
			 dtMeshTile* parentTile = 0;
			 dtPoly* parentPoly = 0;
			if (bestNode.pidx)
				parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
			if (parentRef)
				m_nav.getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);

			// Hit test walls.
			for (int i = 0, j = (int)bestPoly.vertCount-1; i < (int)bestPoly.vertCount; j = i++)
			{
				// Skip non-solid edges.
				if (bestPoly.neis[j] & DT_EXT_LINK)
				{
					// Tile border.
					bool solid = true;
					for (unsigned int k = bestPoly.firstLink; k != DT_NULL_LINK; k = bestTile.links[k].next)
					{
						 dtLink* link = &bestTile.links[k];
						if (link.edge == j)
						{
							if (link.ref != 0)
							{
								 dtMeshTile* neiTile = 0;
								 dtPoly* neiPoly = 0;
								m_nav.getTileAndPolyByRefUnsafe(link.ref, &neiTile, &neiPoly);
								if (filter.passFilter(link.ref, neiTile, neiPoly))
									solid = false;
							}
							break;
						}
					}
					if (!solid) continue;
				}
				else if (bestPoly.neis[j])
				{
					// Internal edge
					 unsigned int idx = (unsigned int)(bestPoly.neis[j]-1);
					 dtPoly ref = m_nav.getPolyRefBase(bestTile) | idx;
					if (filter.passFilter(ref, bestTile, &bestTile.polys[idx]))
						continue;
				}

				// Calc distance to the edge.
				 float* vj = &bestTile.verts[bestPoly.verts[j]*3];
				 float* vi = &bestTile.verts[bestPoly.verts[i]*3];
				float tseg;
				float distSqr = dtDistancePtSegSqr2D(centerPos, vj, vi, tseg);

				// Edge is too far, skip.
				if (distSqr > radiusSqr)
					continue;

				// Hit wall, update radius.
				radiusSqr = distSqr;
				// Calculate hit pos.
				hitPos[0] = vj[0] + (vi[0] - vj[0])*tseg;
				hitPos[1] = vj[1] + (vi[1] - vj[1])*tseg;
				hitPos[2] = vj[2] + (vi[2] - vj[2])*tseg;
			}

			for (unsigned int i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
			{
				 dtLink* link = &bestTile.links[i];
				dtPoly neighbourRef = link.ref;
				// Skip invalid neighbours and do not follow back to parent.
				if (!neighbourRef || neighbourRef == parentRef)
					continue;

				// Expand to neighbour.
				 dtMeshTile* neighbourTile = 0;
				 dtPoly* neighbourPoly = 0;
				m_nav.getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);

				// Skip off-mesh connections.
				if (neighbourPoly.getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
					continue;

				// Calc distance to the edge.
				 float* va = &bestTile.verts[bestPoly.verts[link.edge]*3];
				 float* vb = &bestTile.verts[bestPoly.verts[(link.edge+1) % bestPoly.vertCount]*3];
				float tseg;
				float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);

				// If the circle is not touching the next polygon, skip it.
				if (distSqr > radiusSqr)
					continue;

				if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
					continue;

				dtNode* neighbourNode = m_nodePool.getNode(neighbourRef);
				if (!neighbourNode)
				{
					status |= DT_OUT_OF_NODES;
					continue;
				}

				if (neighbourNode.flags & DT_NODE_CLOSED)
					continue;

				// Cost
				if (neighbourNode.flags == 0)
				{
					getEdgeMidPoint(bestRef, bestPoly, bestTile,
									neighbourRef, neighbourPoly, neighbourTile, neighbourNode.pos);
				}

				 float total = bestNode.total + dtVdist(bestNode.pos, neighbourNode.pos);

				// The node is already in open list and the new result is worse, skip.
				if ((neighbourNode.flags & DT_NODE_OPEN) && total >= neighbourNode.total)
					continue;

				neighbourNode.id = neighbourRef;
				neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
				neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
				neighbourNode.total = total;

				if (neighbourNode.flags & DT_NODE_OPEN)
				{
					m_openList.modify(neighbourNode);
				}
				else
				{
					neighbourNode.flags |= DT_NODE_OPEN;
					m_openList.push(neighbourNode);
				}
			}
		}

		// Calc hit normal.
		dtVsub(hitNormal, centerPos, hitPos);
		dtVnormalize(hitNormal);

		*hitDist = sqrtf(radiusSqr);

		return status;
	}*/

	public boolean isValidPolyRef(dtPoly ref, dtQueryFilter filter)
	{
		dtMeshTile tile[] = new dtMeshTile[1];
		dtPoly poly[] = new dtPoly[1];
		dtStatus status = m_nav.getTileAndPolyByRef(ref, tile, poly);
		// If cannot get polygon, assume it does not exists and boundary is invalid.
		if (dtStatus.dtStatusFailed(status))
			return false;
		// If cannot pass filter, assume flags has changed and boundary is invalid.
		if (!filter.passFilter(ref, tile, poly))
			return false;
		return true;
	}

	/// @par
	///
	/// The closed list is the list of polygons that were fully evaluated during
	/// the last navigation graph search. (A* or Dijkstra)
	///
	/*bool isInClosedList(dtPoly ref) 
	{
		if (!m_nodePool) return false;
		 dtNode* node = m_nodePool.findNode(ref);
		return node && node.flags & DT_NODE_CLOSED;
	}*/

	public dtStatus getEdgeMidPoint(dtPoly from, dtPoly fromPoly, dtMeshTile fromTile,
									dtPoly to, dtPoly toPoly, dtMeshTile toTile,
									float[] mid)
	{
		float left[] = new float[3], right[] = new float[3];
		if (dtStatus.dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
			return new dtStatus(dtStatus.DT_FAILURE | dtStatus.DT_INVALID_PARAM);
		mid[0] = (left[0] + right[0]) * 0.5f;
		mid[1] = (left[1] + right[1]) * 0.5f;
		mid[2] = (left[2] + right[2]) * 0.5f;
		return new dtStatus(dtStatus.DT_SUCCESS);
	}
}
